# coding:utf-8
from child import Ui_child1
import Ui_agv,sys,os,time,re
import struct,math,serial,threading,socket,json
import numpy as np
# import random
from PyQt6 import QtCore, QtWidgets
# from PyQt6 import QtGui
from PyQt6.QtCore import *
from PyQt6.QtCore import pyqtSignal,QTimer,QObject,QThread,QDateTime
from PyQt6.QtWidgets import *
# from PyQt6.QtWidgets import QGraphicsEllipseItem
from PyQt6.QtWidgets import QComboBox, QMessageBox,QFileDialog,QWidget,QApplication, QMainWindow
from PyQt6.QtGui import QIcon,QKeySequence,QBrush
# from PyQt6.QtGui import QPainter, QColor,QPen
# from PyQt6.QtChart import QScatterSeries, QChart, QChartView
from PyQt6.QtSerialPort import QSerialPortInfo
# from qt_material import apply_stylesheet
from src.lib.crc import SumCheck,mycrc
# from my_thread import stop_thread
from datetime import datetime
import pyqtgraph as pg
import pymysql

def extract_ip():#获取本机IP
    st = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        st.connect(('10.255.255.255', 1))
        IP = st.getsockname()[0]
    except Exception:
        IP = '127.0.0.1'
    finally:
        st.close()
    return IP
def get_time():#获取本机时间
        current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
        return current_time
def HexStrAddSpace(HexStr):#十六进制字符串标准化打印
    out_data = ''
    for i,s in enumerate(HexStr):
        if((i%2 == 0)&(i>0)):
            out_data += ' '
            out_data += s
        else:
            out_data += s
    return out_data.upper()
def listToFloat(list):#字符列表转
    return round(struct.unpack('<f', struct.pack('4B', *list))[0],5)
def floatToList(float_value):#小数转字符列表
    # 将浮点数转换为二进制表示
    # binary = struct.pack('!f', float_value)#参数'!f'表示使用大端字节序
    binary = struct.pack('f', float_value)
    # 将二进制表示转换为十六进
    hex_array = [int(b) for b in binary]#int整数表示  hex字符表示
    return hex_array
# 角度曲线绘制
class AngleCurve(QWidget):
    def __init__(self, parent=None):
        super().__init__(parent)
        #添加布局
        self.layout=QGridLayout()
        self.setLayout(self.layout)
        self.pw = pg.PlotWidget(title="AGV 角度曲线",background='w')
        self.pw.showGrid(x=True, y=True, alpha=0.5)
        self.layout.addWidget(self.pw)

        self.plot = self.pw.plot(symbol='o',symbolBrush='r', pen='r',)
        self.x = np.linspace(0, 10, 100)  # x轴数据
        self.y = np.random.rand(100)  # 随机生成一些初始数据
        self.plot.setData(self.x, self.y)
        self.plot1 = self.pw.plot(symbol='h',symbolBrush='b', pen='b',)
        self.y1 =  [item+2 for item in self.y]
        self.plot1.setData(self.x, self.y1)
# 规划地图绘制
class DraggablePoint(QGraphicsEllipseItem):
    def __init__(self, x, y):
        super().__init__(x - 1, y - 1, 3, 3)
        self.setFlag(QGraphicsEllipseItem.ItemIsMovable, True)
        self.setFlag(QGraphicsEllipseItem.ItemIsSelectable, True)

    def mouseMoveEvent(self, event):
        if event.buttons() == Qt.LeftButton:
            
            # pos=event.scenePos()
            # pos=event.pos()
            # pos=self.mapToScene(self.rect().center())
            pos=self.rect().center()
            self.setPos(event.pos()+pos)
            # pos=self.pw.pos()
            # .mapFromScene()
            print(pos)
class CustomInputDialog(QDialog):#自定义输入框
    def __init__(self, parent=None):
        super().__init__(parent)
        self.setWindowTitle("修改坐标")
        layout = QGridLayout(self)
        self.label = QLabel("请输入坐标（mm）:", self)
        self.label1 = QLabel("X:", self)
        self.label2 = QLabel("Y:", self)
        self.line_edit_x = QLineEdit(self)
        self.line_edit_y = QLineEdit(self)
        self.button_box = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Cancel, self)
        
        layout.addWidget(self.label, 0, 0, 1, 2)
        layout.addWidget(self.label1, 1, 0, 1, 1)#添加到几行几列，并占据几行几列的位置)
        layout.addWidget(self.line_edit_x, 1, 1, 1, 1)
        layout.addWidget(self.label2, 2, 0, 1, 1)
        layout.addWidget(self.line_edit_y, 2, 1, 1, 1)
        layout.addWidget(self.button_box, 3, 0, 1, 2)
        
        self.button_box.accepted.connect(self.accept)
        self.button_box.rejected.connect(self.reject)

    def text(self):
        return [float(self.line_edit_x.text()), float(self.line_edit_y.text())]
class PlanMap(QWidget):
    def __init__(self, parent=None):
        super().__init__(parent)
        #添加布局
        self.layout=QGridLayout()
        self.setLayout(self.layout)
        #设置图表
        self.pw = pg.PlotWidget(title="AGV 地图规划",background='w')
        self.layout.addWidget(self.pw)
        self.pw.setXRange(-3, 3)
        self.pw.setYRange(-3, 3)
        self.pw.showGrid(x=True, y=True, alpha=0.5)
        
        #添加点
        # self.point1 = DraggablePoint(5, 5)
        # self.pw.addItem(self.point1)

        #添加散点
        self.scatter = pg.ScatterPlotItem(
            size=10, 
            brush= pg.mkBrush('g'),
            pen=pg.mkPen(color='g'),
            # pxMode=False, 
            hoverable=True,
            hoverPen=pg.mkPen('r'),
            hoverBrush=pg.mkBrush('r'),
            hoverSize=10
            )
        self.pw.addItem(self.scatter)
        self.dotdata = []
        # self.scatter.setData(self.dotdata)

        #绘画示例
        self.pw.plot([5],[5],pen='g', brush=pg.mkBrush('r'),symbol='o')#画点
        # self.pw.plot([1,2],[1,2],pen='b')#画线
        baseLine = pg.PolyLineROI([[0, 0], [1, 0], [1.5, 1], [2, 0], [3, 0]], pen=pg.mkPen('g', width=2), movable=True, closed=True)#画区域
        # self.pw.addItem(baseLine)
        

        # 添加曲线项
        self.plot = self.pw.plot(symbol='o', pen='r',)
        # 初始化数据
        self.x = np.linspace(0, 10, 100)  # x轴数据
        self.y = np.random.rand(100)  # 随机生成一些初始数据
        # self.x=[]
        # self.y=[]
        # 更新数据并绘制曲线
        # self.update_plot()

        #鼠标事件
        self.setMouseTracking(False)
        self.plot.scene().sigMouseMoved.connect(self.mouseover)
        # self.point1.mousePressEvent.connect(self.mouseclickPoint)
        # self.scatter.sigMouseMoved.connect(self.mouseover)
        self.plot.scene().sigMouseClicked.connect(self.mouse_clicked)
        self.scatter.sigClicked.connect(self.clicked)

        ## Make all plots clickable
        self.clickedPen = pg.mkPen('red', width=5)
        self.brush=pg.mkBrush(255, 0, 0, 120)
        self.lastClicked = []
        self.flag_editPoint=0
    def mouseclickPoint(self,event):
        print("mouseclickPoint",event)
    def clicked(self,plot, points):
            print(points)
            for p in self.lastClicked:
                p.resetPen()
            for p in points:
                p.setPen(self.clickedPen)
                p.setBrush(self.brush)
                pointNumber=p.data()[0]
                if self.flag_editPoint==2:#删除点
                    del self.dotdata[pointNumber]
                    for index, value in enumerate(self.dotdata):#点重新编号
                        self.dotdata[index]['data'][0]=index
                    
                elif self.flag_editPoint==3:#修改点
                    '''
                    #自带输入框
                    # text, ok = QInputDialog.getText(self, "Input Dialog", "Enter your name:")
                    # if ok:
                    #     print(f"Your name is: {text}")
                    '''
                    dialog = CustomInputDialog()
                    if dialog.exec_() == QDialog.Accepted:
                        user_input = dialog.text()
                        print("用户输入的内容为:", user_input)
                        del self.dotdata[pointNumber]
                        newpoint={'pos': (user_input[0], user_input[1]),'data':[pointNumber,1]}
                        self.dotdata.insert(pointNumber,newpoint)
                self.scatter.setData(self.dotdata)
                # print(p.pos())
            self.lastClicked = points
    def update_plot(self):
        # 更新数据
        self.y[:-1] = self.y[1:]  # 数据向前移动一个位置
        self.y[-1] = np.random.rand()  # 在最后一个位置生成随机数

        # 更新曲线的数据
        self.plot.setData(self.x, self.y)

        # 设置曲线在x轴上的范围
        # self.pw.setXRange(self.x[0], self.x[-1])

        # 更新绘图
        self.pw.update()

        # 每隔一段时间调用update_plot函数来更新数据和绘图
        pg.QtCore.QTimer.singleShot(100, self.update_plot)
    def mouseover(self,pos):    
        act_pos = self.plot.mapFromScene(pos)# 参数pos 是像素坐标，需要 转化为  刻度坐标
        if type(act_pos) != QtCore.QPointF:
            return
        # print("over_1:",act_pos.x(), act_pos.y())
        self.new_point_x = round(act_pos.x(),2)
        self.new_point_y = round(act_pos.y(),2)

    def mouse_clicked(self,event):
        
        if self.flag_editPoint==1:
            newpoint={'pos': (self.new_point_x , self.new_point_y),'data':[len(self.dotdata),1]}
            if newpoint not in self.dotdata: #点已经存在则不添加
                self.dotdata.append(newpoint)   
                self.scatter.setData(self.dotdata)
        # elif self.flag_editPoint==2:
        #     self.dotdata.pop(self.lastClicked)
        print("my position is:",self.new_point_x,self.new_point_y)
# 运动轨迹绘制
class PaintRoute(QWidget):
    def __init__(self, parent=None):
        super().__init__(parent)
        #添加布局
        self.layout=QGridLayout()
        self.setLayout(self.layout)
        self.pw = pg.PlotWidget(title="AGV 运动轨迹",background='w')
        self.layout.addWidget(self.pw)

        
        

        # self.scatter = pg.ScatterPlotItem(size=10, brush= pg.mkBrush('g'),pen=pg.mkPen(color='g'))
        self.scatter = pg.ScatterPlotItem(
            size=10, 
            brush= pg.mkBrush('g'),
            pen=pg.mkPen(color='g'),
            # pxMode=False, 
            hoverable=True,
            hoverPen=pg.mkPen('r'),
            hoverBrush=pg.mkBrush('r'),
            hoverSize=10
            )
        self.pw.addItem(self.scatter)
        #设置图表
        self.pw.setXRange(-3, 3)
        self.pw.setYRange(-3, 3)
        self.pw.showGrid(x=True, y=True, alpha=0.5)

        #绘画示例
        self.dotdata = []  # 设置每个点的颜色
        # self.scatter.setData(self.dotdata)

        # self.pw.plot([5],[5],pen='g', brush=pg.mkBrush('r'),symbol='o')
        # self.pw.plot([1,2],[1,2],pen='b')
        # baseLine = pg.PolyLineROI([[0, 0], [1, 0], [1.5, 1], [2, 0], [3, 0]], pen=(0, 255, 0, 100), movable=False)
        # self.pw.addItem(baseLine)

        # 创建一个曲线项
        self.plot = self.pw.plot(symbol='o', pen='r',)
        # 初始化数据
        # self.data = np.random.rand(100)  # 随机生成一些初始数据
        # self.x = np.linspace(0, 10, 100)  # x轴数据
        self.x=[]
        self.y=[]
        # 更新数据并绘制曲线
        # self.update_plot()

        #鼠标事件
        self.setMouseTracking(False)
        self.plot.scene().sigMouseMoved.connect(self.mouseover)
        self.plot.scene().sigMouseClicked.connect(self.mouse_clicked)
        self.scatter.sigClicked.connect(self.clicked)
        ## Make all plots clickable
        self.clickedPen = pg.mkPen('red', width=5)
        self.brush=pg.mkBrush(255, 0, 0, 120)
        self.lastClicked = []
    def clicked(self,plot, points):
            
            # for p in self.lastClicked:
            #     p.resetPen()
                # print("clicked points", p.pos())
            for p in points:
                p.setPen(self.clickedPen)
                p.setBrush(self.brush)
                print(p.data())
            self.lastClicked = points
    def update_plot(self):
        # 更新数据
        # self.data[:-1] = self.data[1:]  # 数据向前移动一个位置
        # self.data[-1] = np.random.rand()  # 在最后一个位置生成随机数

        # 更新曲线的数据
        self.plot.setData(self.x, self.data)

        # 设置曲线在x轴上的范围
        # self.pw.setXRange(self.x[0], self.x[-1])

        # 更新绘图
        self.pw.update()

        # 每隔一段时间调用update_plot函数来更新数据和绘图
        pg.QtCore.QTimer.singleShot(100, self.update_plot)
    def mouseover(self,pos):    
        act_pos = self.plot.mapFromScene(pos)# 参数pos 是像素坐标，需要 转化为  刻度坐标
        if type(act_pos) != QtCore.QPointF:
            return
        # print("over_1:",act_pos.x(), act_pos.y())
        self.new_point_x = round(act_pos.x(),2)
        self.new_point_y = round(act_pos.y(),2)

    def mouse_clicked(self,event):
        if self.new_point_x not in self.x: 
            self.x.append(self.new_point_x)
        else:
            index = self.x.index(self.new_point_x)  
        if self.new_point_y not in self.y: 
            self.y.append(self.new_point_y)
        newpoint={'pos': (self.new_point_x , self.new_point_y),'data':[1,1]}
        if newpoint not in self.dotdata: #点已经存在则不添加
            self.dotdata.append(newpoint)   
        self.scatter.setData(self.dotdata)
        print("my position is:",self.new_point_x,self.new_point_y)
# 重写下拉框点(击时获取串口列表)
class MyComboBox(QComboBox):
    def __init__(self, parent=None):
        super().__init__(parent)

    def showPopup(self):
        name = self.currentText()
        self.clear()
        serials = QSerialPortInfo.availablePorts()
        for com in serials:
            self.addItem(com.portName() + ' ' + com.description())
        super().showPopup()
        index = self.findText(name)
        if index >= 0:
            self.setCurrentIndex(index)
# 定义信号
class signalclass(QObject):   
    print = pyqtSignal(str) 
    verify_recieveData = pyqtSignal(bytes) 
    process_PGVData = pyqtSignal(bytes) 
    # update_time=pyqtSignal(str)
    show_msg = pyqtSignal(str)
# 定义进程
class thread_class(QThread):
    update_date = pyqtSignal(str)
    def run(self):    # 该类模拟后台进程更新时间
        while True:
            data = QDateTime.currentDateTime()
            currTime = data.toString("yyyy-MM-dd hh:mm:ss")
            try:
                self.update_date.emit(currTime)
            except:
                break
            time.sleep(1)
#定义数据结构体
class PGV(dict):   
    def __getattr__(self, key):
        return self[key]

    def __setattr__(self, key, value):
        self[key] = value

# 主窗口      
class Main(QtWidgets.QMainWindow,Ui_agv.Ui_MainWindow):
    def __init__(self):
        super(Main, self).__init__()
        self.setupUi(self)
        self.setFocusPolicy(Qt.FocusPolicy.StrongFocus)  # 设置获取焦点的方法

        #region # 子窗口       
        self.ch = Child()#弹出窗口
        # self.btn_map.clicked.connect(self.ch.OPEN)

        self.paint = PaintRoute()#绘图窗口
        self.map = PlanMap()
        self.angleCurve = AngleCurve()
        self.layout_route.addWidget(self.paint)
        self.layout_map.addWidget(self.map)
        self.layout_angleCurve.addWidget(self.angleCurve)
        # self.gridLayout.addWidget(self.paint, 2, 9, 3, 5)#添加到几行几列，并占据几行几列的位置
        #endregion
        #region # 定义信号
        self.signal=signalclass()
        self.signal.print[str].connect(self.signalCall_print)
        self.signal.verify_recieveData.connect(self.signalCall_verify_recieveData)
        self.signal.process_PGVData.connect(self.signalCall_process_PGVData)
        self.signal.show_msg[str].connect(self.signalCall_show_msg)
        self.note=QDialog()
        self.pgv=PGV()
        #endregion
        #region # 定义进程
        self.thread = thread_class()
        self.thread.update_date.connect(self.signalCall_update_time)
        self.thread.start() 
        #endregion
        #region # 定义定时器 
        self.timer0_deviceState = QTimer()
        self.timer0_deviceState.timeout.connect(self.fuc_update_deviceState)
        self.timer0_deviceState.start(1000)
        self.timer1_agv_stop = QTimer()
        self.timer1_agv_stop.timeout.connect(self.fuc_agv_autoStop)
        # self.timer_agv_stopSingleMove = QTimer()
        # self.timer_agv_stopSingleMove.timeout.connect(self.fuc_agv_autoStopSingleMove)
        self.timer2_agv_moveCycle = QTimer()
        self.timer2_agv_moveCycle.timeout.connect(self.fuc_agv_cycleMove)
        self.timer3_camera_query = QTimer()
        self.timer3_camera_query.timeout.connect(self.fuc_camera_query)
        self.timer4_imu_query = QTimer()
        self.timer4_imu_query.timeout.connect(lambda:self.fuc_imu_config("queryAngle"))
        self.timer5_agv_angleJudge = QTimer()
        self.timer5_agv_angleJudge.timeout.connect(self.fuc_agv_angleJudge)
        self.timer6_agv_odometer = QTimer()
        self.timer6_agv_odometer.timeout.connect(self.fuc_agv_odometer)
        self.timer7_agv_toDistance = QTimer()
        self.timer7_agv_toDistance.timeout.connect(self.fuc_agv_toDistance)
        # self.timer8_agv_anologAngle = QTimer()
        # self.timer8_agv_anologAngle.timeout.connect(self.fuc_agv_anologAngle)
        self.timer9_playPoints = QTimer()
        self.timer9_playPoints.timeout.connect(self.task_playPoints)
        self.timer10_imuTest = QTimer()
        self.timer10_imuTest.timeout.connect(self.task_imuTest)
        self.timer11_update_mapPoints = QTimer()
        self.timer11_update_mapPoints.timeout.connect(self.task_update_mapPoints)
        #endregion
        

        #region # 数据库        
        self.dbConfig={
            "host":'localhost',
            "port":3306,
            "user":"root",
            "password":"root",
            "database":"agv",
            'connect_timeout': 0.3  # 设置连接超时时间为0.3秒
        }
        self.tableName="test"
        self.btn_database_connect.clicked.connect(self.fuc_database_connect)      
        self.btn_database_add.clicked.connect(self.fuc_database_add) 
        self.btn_database_delete.clicked.connect(self.fuc_database_delete) 
        self.btn_database_update.clicked.connect(self.fuc_database_update)
        self.btn_database_query.clicked.connect(self.fuc_database_query) 
        # self.btn_database_query_all.clicked.connect(self.fuc_database_query_all) 

        self.fuc_database_connect()  
        #endregion
        #region # 参数配置_上位机
        self.cnt_deviceState=0
        self.last_agv_currentPosX=1
        self.last_agv_currentPosY=1        
        #endregion
        #region # 控制板配置
        self.btn_system_restart.clicked.connect(self.fuc_system_restart)
        self.btn_system_reset.clicked.connect(self.fuc_system_reset)
        self.btn_read_flashParameter.clicked.connect(self.fuc_read_flashParameter)
        self.btn_set_flashParameter.clicked.connect(self.fuc_set_flashParameter)
        self.btn_save_flashParameter.clicked.connect(self.fuc_save_flashParameter)
        #endregion
        #region # 服务器通信配置
        self.cmdDic={'01':'GO','02':'GP',"03":'GD',"04":'RV',"05":'UP'}
        self.check_cmd=[self.check_cmd_1,self.check_cmd_2,self.check_cmd_3,self.check_cmd_4,self.check_cmd_5,self.check_cmd_6,self.check_cmd_7,self.check_cmd_8,self.check_cmd_9,self.check_cmd_10]
        self.com_num=[self.com_cmd_num_1,self.com_cmd_num_2,self.com_cmd_num_3,self.com_cmd_num_4,self.com_cmd_num_5,self.com_cmd_num_6,self.com_cmd_num_7,self.com_cmd_num_8,self.com_cmd_num_9,self.com_cmd_num_10]
        self.com_name=[self.com_cmd_name_1,self.com_cmd_name_2,self.com_cmd_name_3,self.com_cmd_name_4,self.com_cmd_name_5,self.com_cmd_name_6,self.com_cmd_name_7,self.com_cmd_name_8,self.com_cmd_name_9,self.com_cmd_name_10]
        self.input_p1=[self.input_cmd_1_p_1,self.input_cmd_2_p_1,self.input_cmd_3_p_1,self.input_cmd_4_p_1,self.input_cmd_5_p_1,self.input_cmd_6_p_1,self.input_cmd_7_p_1,self.input_cmd_8_p_1,self.input_cmd_9_p_1,self.input_cmd_10_p_1]
        self.input_p2=[self.input_cmd_1_p_2,self.input_cmd_2_p_2,self.input_cmd_3_p_2,self.input_cmd_4_p_2,self.input_cmd_5_p_2,self.input_cmd_6_p_2,self.input_cmd_7_p_2,self.input_cmd_8_p_2,self.input_cmd_9_p_2,self.input_cmd_10_p_2]
        self.input_p3=[self.input_cmd_1_p_3,self.input_cmd_2_p_3,self.input_cmd_3_p_3,self.input_cmd_4_p_3,self.input_cmd_5_p_3,self.input_cmd_6_p_3,self.input_cmd_7_p_3,self.input_cmd_8_p_3,self.input_cmd_9_p_3,self.input_cmd_10_p_3]
        self.btn_cmd=[self.btn_sendCmd_1,self.btn_sendCmd_2,self.btn_sendCmd_3,self.btn_sendCmd_4,self.btn_sendCmd_5,self.btn_sendCmd_6,self.btn_sendCmd_7,self.btn_sendCmd_8,self.btn_sendCmd_9,self.btn_sendCmd_10]
        self.label_cmdState=[self.label_cmdState_1,self.label_cmdState_2,self.label_cmdState_3,self.label_cmdState_4,self.label_cmdState_5,self.label_cmdState_6,self.label_cmdState_7,self.label_cmdState_8,self.label_cmdState_9,self.label_cmdState_10]
        # self.com_num=['self.com_cmd_num_'+str(n) for n in range(1,10)]
        # self.num=[1,2,3,4,5,6,7,8,9,10]
        for i in range(10):
            self.com_num[i].clear()
            self.com_num[i].addItems(list(self.cmdDic.keys()))
            self.com_num[i].currentIndexChanged.connect(self.fuc_cmdNumChange)
            self.com_name[i].clear()
            self.com_name[i].addItems(list(self.cmdDic.values()))
            self.com_name[i].currentIndexChanged.connect(self.fuc_cmdNameChange)
            self.input_p1[i].setText('1000')
            self.input_p2[i].setText('1000')
            self.input_p3[i].setText('1000')
            self.btn_cmd[i].clicked.connect(self.fuc_sendCmd)
        self.btn_cmd_save.clicked.connect(self.fuc_cmdSave)
        self.btn_cmd_load.clicked.connect(self.fuc_cmdLoad)

        self.flag_server=0#服务器开启标志
        self.cmd_count=0#批量发送的指令条数
        self.input_ip.setText(extract_ip())
        self.btn_server.clicked.connect(self.server_open)
        self.btn_query_ip.clicked.connect(self.query_ip)
        self.check_cmd_all.toggled.connect(self.fuc_check_cmd_all)
        self.btn_sendCmd_all.clicked.connect(self.fuc_sendCmd_all)
        self.btn_connect_server.clicked.connect(self.connect_server)
        self.check_mode_wifiToUart.toggled.connect(self.fuc_check_mode_wifiToUart)
        # self.com_server.itemsChanged.connect(self.fuc_com_server_change)
        self.com_server.currentIndexChanged.connect(self.fuc_com_server_change)
        #endregion

        #region # 参数配置_电机
        self.canOpenDic={'3000':['电机转速',0x10],'3001':['电机绝对位置',0x20],"3009":['母线电压',0x10]}
        self.indexLenth={0x10:2,0x20:4}# 08表示索引为1个字节，10表示索引为2个字节，20表示索引为4个字节
        self.mapNumber=0 #映射个数
        self.com_motor_map1_1.clear()
        self.com_motor_map1_1.addItems(list(self.canOpenDic.keys()))
        self.flag_motor_nodeId_2_isCongfiged=0
        #endregion
        #region # 参数配置_AGV小车
        self.agv_wheelDistance=54
        self.agv_moveState="stop"#['stop','toPos1_rotate',"toPos2_forward"]
        self.agv_rotateDirection="CW"#["CW","CCW"]
        self.agv_countCycle=0
        self.agv_flag_moveCycle=0

        v=float(self.input_agv_parameter3_realSpeed.text())
        reductionRatio=float(self.input_agv_parameter2_reductionRatio.text())
        wheelSize=float(self.input_agv_parameter1_wheelSize.text())
        rpm=v*60*reductionRatio/wheelSize/10/math.pi
        rpm=int(rpm)
        rateSpeed=round(v/math.pi/self.agv_wheelDistance/10*360,2)

        self.input_2motor_speed_1.setText(str(rpm))
        self.input_2motor_speed_2.setText(str(rpm))
        self.input_agv_parameter4_rateSpeed.setText(str(rateSpeed))
        self.input_agv_parameter3_realSpeed.editingFinished.connect(lambda:self.fuc_agv_speedChange(0))
        self.input_2motor_speed_1.editingFinished.connect(lambda:self.fuc_agv_speedChange(1))
        # self.input_imu_angleDeviation.returnPressed.connect(self.test)
        # self.input_imu_angleDeviation.textEdited.connect(self.test)
        # self.input_imu_angleDeviation.editingFinished.connect(self.test)
        #endregion

        #region # 日志/IAP远程升级
        data_today = datetime.now().strftime('20%y-%m-%d')
        dir_name = 'log'
        if not os.path.isdir(dir_name):
            os.makedirs(dir_name)
        # current_dir = os.getcwd()
        # print(current_dir)
        self.input_record_name.setText(data_today)
        self.check_record.setChecked(True)
        self.btn_openlog.clicked.connect(self.fuc_openlog)
        # IAP远程升级
        self.btn_iap_in.clicked.connect(self.fuc_iap_in)
        self.btn_app_in.clicked.connect(self.fuc_app_in)
        self.btn_receive_in.clicked.connect(self.fuc_receive_in)
        self.btn_iap_sendBin.clicked.connect(self.fuc_iap_sendBin)#串口升级
        self.btn_iap_in_2.clicked.connect(self.fuc_iap_in_2)
        self.btn_iap_sendBin_2.clicked.connect(self.fuc_iap_sendBin_2)#网口升级
        # self.btn_iap_complete.clicked.connect(self.fuc_iap_complete)
        self.iap_filename=''
        #endregion

        
        #region # 串口配置
        self.serial_cfg()
        #endregion
        #region # 控制声光
        self.check_updateAudio.toggled.connect(self.fuc_updateAudio)
        self.btn_clear.clicked.connect(self.fuc_clear)
        self.btn_playVoice.clicked.connect(self.fuc_playVoice)
        self.com_voiceList.currentIndexChanged.connect(self.fuc_playVoice)
        # self.slider_volume.sliderReleased.connect(self.fuc_changeVolume)
        self.slider_volume.sliderMoved.connect(self.fuc_changeVolume)
        self.slider_volume.sliderPressed.connect(self.fuc_changeVolume)
        self.btn_pauseVoice.clicked.connect(self.fuc_pauseVoice)
        self.check_ledSwitch.toggled.connect(self.fuc_ledSwitch)
        self.check_ledsSwitch.toggled.connect(self.fuc_ledsSwitch)
        self.btn_ledBlink.clicked.connect(self.fuc_ledBlink)
        self.btn_ledsBlink.clicked.connect(self.fuc_ledsBlink)
        #endregion
        #region # 控制IO扩展模块
        self.check_output_1.toggled.connect(lambda:self.fuc_outputControl(0))
        self.check_output_2.toggled.connect(lambda:self.fuc_outputControl(1))
        self.check_output_3.toggled.connect(lambda:self.fuc_outputControl(2))
        self.check_output_4.toggled.connect(lambda:self.fuc_outputControl(3))
        self.check_output_5.toggled.connect(lambda:self.fuc_outputControl(4))
        self.check_output_6.toggled.connect(lambda:self.fuc_outputControl(5))
        self.check_output_7.toggled.connect(lambda:self.fuc_outputControl(6))
        self.check_output_8.toggled.connect(lambda:self.fuc_outputControl(7))
        #endregion

        #region # 控制AGV小车
        self.btn_agv_stop.clicked.connect(self.fuc_agv_stop)
        self.btn_agv_rotateCW.clicked.connect(self.fuc_agv_rotateCW)
        self.btn_agv_rotateCCW.clicked.connect(self.fuc_agv_rotateCCW)
        self.btn_agv_moveForward.clicked.connect(self.fuc_agv_moveForward)#向前移动一段时间
        self.btn_agv_moveBack.clicked.connect(self.fuc_agv_moveBack)#向后移动一段时间
        self.btn_agv_toAngle.clicked.connect(self.fuc_agv_toAngle)#旋转到指定角度
        self.btn_agv_toDistance.clicked.connect(self.fuc_agv_toDistance)#运行一段距离
        self.btn_agv_toPos.clicked.connect(self.fuc_agv_toPos)#运行到指定点
        self.btn_agv_resetPos.clicked.connect(self.fuc_agv_resetPos)
        self.btn_agv_run.clicked.connect(self.fuc_agv_run)
        self.btn_agv_runBack.clicked.connect(self.fuc_agv_runBack)



        self.btn_2motor_cycleMove.clicked.connect(self.fuc_agv_cycleMove)
        self.btn_2motor_stopCycle.clicked.connect(self.fuc_agv_stopCycle)
        self.btn_2motor_countinueCylcle.clicked.connect(self.fuc_agv_cycleMove)
       
        #endregion
        #region # 无线手柄控制
        self.handle_LU.clicked.connect(self.fuc_handle_LU)
        self.handle_LD.clicked.connect(self.fuc_handle_LD)
        self.handle_LL.clicked.connect(self.fuc_handle_LL)
        self.handle_LR.clicked.connect(self.fuc_handle_LR)
        self.handle_SE.clicked.connect(self.fuc_handle_SE)
        self.handle_ST.clicked.connect(self.fuc_handle_ST)

        self.handle_RU.clicked.connect(self.fuc_handle_RU)
        self.handle_RD.clicked.connect(self.fuc_handle_RD)
        self.handle_RL.clicked.connect(self.fuc_handle_RL)
        self.handle_RR.clicked.connect(self.fuc_handle_RR)

        self.handle_LK.clicked.connect(self.fuc_handle_LK)
        self.handle_RK.clicked.connect(self.fuc_handle_RK)

        self.handle_AL1.sliderReleased.connect(self.fuc_handle_AL1)
        self.handle_AL2.sliderReleased.connect(self.fuc_handle_AL2)
        self.handle_AR1.sliderReleased.connect(self.fuc_handle_AR1)
        self.handle_AR2.sliderReleased.connect(self.fuc_handle_AR2)

        self.handle_LD.pressed.connect(self.fuc_handle_LD_pressed)
        #endregion
        #region # 多点路径规划
        # self.btn_agv_queryPoints.clicked.connect(self.fuc_agv_queryPoints)
        # self.btn_setPoint.clicked.connect(self.fuc_setPoint)
        self.check_pointCycle.clicked.connect(self.fuc_pointCycle)
        self.btn_startPointRecycle.clicked.connect(self.fuc_startPointRecycle)
        #endregion
        #region # 地图规划
        dir_name = 'mapPoints'
        if not os.path.isdir(dir_name):
            os.makedirs(dir_name)
        self.btn_add_mapPoint.clicked.connect(self.fuc_add_mapPoint)
        self.btn_remove_mapPoint.clicked.connect(self.fuc_remove_mapPoint)
        self.btn_edit_mapPoint.clicked.connect(self.fuc_edit_mapPoint)

        self.btn_save_mapPoints.clicked.connect(self.fuc_save_mapPoints)
        self.btn_load_mapPoints.clicked.connect(self.fuc_load_mapPoints)
        self.btn_clear_mapPoints.clicked.connect(self.fuc_clear_mapPoints)

        self.map.dotdataNum=0
        self.map.dotdataCnt=0
        self.btn_run_mapPoints.clicked.connect(lambda:self.fuc_run_mapPoints(1))
        self.btn_runBack_mapPoints.clicked.connect(lambda:self.fuc_run_mapPoints(0))

        #endregion
        #region # 运动轨迹
        dir_name = 'trajectory'
        if not os.path.isdir(dir_name):
            os.makedirs(dir_name)
        # self.paint.dotdata=[{'pos': [-676.1, 0.0]},{'pos': [0, 0]}]#初始化轨迹点
        self.paint.scatter.setData(self.paint.dotdata)
        self.dotdata=[]
        self.btn_clear_points.clicked.connect(self.fuc_clear_points)
        self.btn_save_points.clicked.connect(self.fuc_saveTrajectory)
        self.btn_load_points.clicked.connect(self.fuc_loadTrajectory)
        self.btn_play_points.clicked.connect(lambda:self.fuc_playTrajectory(1))
        self.btn_playback_points.clicked.connect(lambda:self.fuc_playTrajectory(0))
        self.btn_save_keyPoints.clicked.connect(self.fuc_save_keyDot)
        self.btn_load_keyPoints.clicked.connect(self.fuc_load_keyDot)

        self.keyDots=[]#关键点
        #endregion
        #region # 配置电机
        self.btn_motor_active.clicked.connect(lambda:self.fuc_motors_config("active"))
        self.btn_motor_setMode.clicked.connect(lambda:self.fuc_motors_config("setMode"))
        self.btn_motor_clearError.clicked.connect(lambda:self.fuc_motors_config("clearError"))
        self.btn_motor_prepareServe.clicked.connect(lambda:self.fuc_motors_config("prepareServe"))
        self.btn_motor_waitEn.clicked.connect(lambda:self.fuc_motor_preConfig("waitEn"))
        self.btn_motor_en.clicked.connect(lambda:self.fuc_motor_preConfig("en"))
        self.check_motor_en.toggled.connect(lambda:self.fuc_motors_config("check_en"))
        self.btn_motor_setSpeedUpTime.clicked.connect(lambda:self.fuc_motors_config("setSpeedUpTime"))
        self.btn_motor_setSpeedDownTime.clicked.connect(lambda:self.fuc_motors_config("setSpeedDownTime"))
        self.btn_motor_setAll.clicked.connect(self.fuc_motors_configAll)

        self.btn_motor_setNMT.clicked.connect(lambda:self.fuc_motors_config("NMT"))
        self.btn_motor_setPDO.clicked.connect(lambda:self.fuc_motors_configPDO(0))
        self.btn_motor_setPDO_2.clicked.connect(lambda:self.fuc_motors_configPDO(1))
        self.btn_motor_setPDO_3.clicked.connect(lambda:self.fuc_motors_configPDO(2))
        self.btn_motor_setPDO_4.clicked.connect(lambda:self.fuc_motors_configPDO(3))

        self.check_motor_enPDO.toggled.connect(lambda:self.fuc_motor_preConfigPDO("PDO_enable" if self.check_motor_enPDO.isChecked() else "PDO_forbid",0))
        self.check_motor_enPDO_2.toggled.connect(lambda:self.fuc_motor_preConfigPDO("PDO_enable" if self.check_motor_enPDO_2.isChecked() else "PDO_forbid",1))
        self.check_motor_enPDO_3.toggled.connect(lambda:self.fuc_motor_preConfigPDO("PDO_enable" if self.check_motor_enPDO_3.isChecked() else "PDO_forbid",2))
        self.check_motor_enPDO_4.toggled.connect(lambda:self.fuc_motor_preConfigPDO("PDO_enable" if self.check_motor_enPDO_4.isChecked() else "PDO_forbid",3))

        self.check_2motor_en_1.toggled.connect(lambda:self.fuc_motor_en(2))
        self.check_2motor_en_2.toggled.connect(lambda:self.fuc_motor_en(3))
        #endregion
        #region # 控制电机
        self.btn_motor_setSpeed.clicked.connect(lambda:self.fuc_motor_setSpeed(0))
        self.btn_motor_stop.clicked.connect(lambda:self.fuc_motor_setSpeed(2))
        self.slider_motor_setSpeed.sliderReleased.connect(lambda:self.fuc_motor_setSpeed(1))

        self.btn_2motor_setSpeed_1.clicked.connect(lambda:self.fuc_2motor_setSpeed1(0))
        self.btn_2motor_stop_1.clicked.connect(lambda:self.fuc_2motor_setSpeed1(2))
        self.slider_2motor_setSpeed_1.sliderReleased.connect(lambda:self.fuc_2motor_setSpeed1(1))

        self.btn_2motor_setSpeed_2.clicked.connect(lambda:self.fuc_2motor_setSpeed2(0))
        self.btn_2motor_stop_2.clicked.connect(lambda:self.fuc_2motor_setSpeed2(2))
        self.slider_2motor_setSpeed_2.sliderReleased.connect(lambda:self.fuc_2motor_setSpeed2(1))
        
        self.btn_2motor_setSpeed.clicked.connect(lambda:self.fuc_2motor_setSpeed(1)) 
        #endregion

        #region # 配置雷达避障
        self.check_obstacle_ignore.toggled.connect(self.fuc_obstacle_ignore)
        #endregion
        #region # 配置IMU（陀螺仪）
        self.btn_imu_setNMT.clicked.connect(lambda:self.fuc_imu_config("NMT"))
        self.btn_imu_queryAngle.clicked.connect(lambda:self.fuc_imu_config("queryAngle"))
        self.check_imu_queryConstant.toggled.connect(self.fuc_imu_queryConstant)
        self.btn_imu_setPDO.clicked.connect(lambda:self.fuc_imu_configPDO(0))
        self.btn_imu_enPDO.clicked.connect(lambda:self.fuc_imu_preConfigPDO("PDO_enable",0))
        self.btn_imu_disablePDO.clicked.connect(lambda:self.fuc_imu_preConfigPDO("PDO_forbid",0))
        self.btn_IMU_setZero.clicked.connect(self.fuc_imu_zero)
        self.btn_IMU_setZero_2.clicked.connect(self.fuc_imu_zero)
        self.btn_angleRectify.clicked.connect(self.fuc_angleRectify)
        # self.btn_imu_setOffset.clicked.connect(self.fuc_imu_setOffset)
        self.btn_imu_setIntegralGain.clicked.connect(self.fuc_imu_setIntegralGain)
        self.flag_imu_test=0
        self.btn_imu_testCW.clicked.connect(lambda:self.fuc_imu_test(1))
        self.btn_imu_testCCW.clicked.connect(lambda:self.fuc_imu_test(0))
        self.btn_imu_test.clicked.connect(lambda:self.fuc_imu_test(2))

        #endregion
        #region # 查询读码相机
        self.btn_camera_query.clicked.connect(self.fuc_camera_query)
        self.check_camera_queryConstant.toggled.connect(self.fuc_camera_queryConstant)
        #endregion
    def test(self):
        # self.input_imu_angleDeviation.setText("0")
        if(self.input_imu_angleDeviation.text()[-1]!='°'):self.input_imu_angleDeviation.setText(self.input_imu_angleDeviation.text()+"°")


        # self.btn_motor_setSpeedUpTime.clicked.connect(self.lambda_test())
    def lambda_test(self):
        print("hlly")
    #region # 数据库
    def fuc_database_connect(self):
        #连接数据库
        try:
            self.db = pymysql.connect(**self.dbConfig)
            self.signal.print.emit("数据库连接成功")

            #使用cursor()方法创建一个游标对象
            self.cursor = self.db.cursor()

            #使用execute()方法执行SQL语句
            self.cursor.execute("SELECT * FROM `test`")

            #使用fetall()获取全部数据
            self.dbData = self.cursor.fetchall()

            #打印获取到的数据
            print(self.dbData[0][1])
            print(type(self.dbData))

            #关闭游标和数据库的连接
            # self.cursor.close()
            # self.db.close()
        except pymysql.Error as e:
            try:
                self.signal.print.emit("数据库连接失败:", str(e))
            except:
                print("数据库连接失败")
    def fuc_database_add(self):  
        print("add")
    def fuc_database_delete(self):
        print("delete")    
    def fuc_database_update(self):
        print("update")
    def fuc_database_query(self):
        print("query")
        self.cursor.execute("SELECT * FROM %s"%self.tableName)
        self.dbData = self.cursor.fetchall()
        self.signal.print.emit(str(self.dbData))
       
    #endregion
    #region # 重写鼠标和按键事件
    def mousePressEvent(self, event):
        if event.button() == Qt.MouseButton.LeftButton:
            print("鼠标左键点击")
        elif event.button() == Qt.MouseButton.RightButton:
            print("鼠标右键点击")
        elif event.button() == Qt.MouseButton.MiddleButton:
            print("鼠标中键点击")
    def keyPressEvent(self, event): #重写了键盘检测事件
        # self.print_constant_name(event.key())
        # print("按下：" + str(event.key()))
        if(event.key() == Qt.Key_Escape):
            print('测试：ESC')
        if(event.key() == Qt.Key_A):
            print('测试：A')
        if(event.key() == Qt.Key_1):
            print('测试：1')
        if(event.key() == Qt.Key.Key_Enter):
            print('测试：Enter')
        if(event.key() == Qt.Key.Key_Return):
            print('测试：Return')
        if(event.key() == Qt.Key_Space):
            print('测试：Space')
            self.fuc_agv_handStop()
        if(event.key() == Qt.Key.Key_Up):
            self.fuc_agv_moveForward()
        if(event.key() == Qt.Key.Key_Down):
            self.fuc_agv_moveBack()
        if(event.key() == Qt.Key.Key_Left):
            self.fuc_agv_handStop()
            time.sleep(1)
            self.fuc_agv_rotateCW()
        if(event.key() == Qt.Key.Key_Right):
            self.fuc_agv_handStop()
            time.sleep(1)
            self.fuc_agv_rotateCCW()
    def print_constant_name(self,key_code):  #根据检测到的键码值打印相应的常量名称
        key_sequence = QKeySequence(key_code)
        constant_name = key_sequence.toString()
        print(constant_name)
    #endregion
    #region # 串口配置
    def serial_cfg(self):
        # 初始化serial对象 用于串口通信
        self.ser = serial.Serial()
        # 初始化端口列表
        # self.portList = MyComboBox()
        # print(self.btn_2motor_setSpeed_2.pos().x())
        # print(self.portList.geometry())
        self.portList = MyComboBox(self.centralwidget)
        self.gridLayout.addWidget(self.portList, 1, 1, 1, 1)#添加到几行几列，并占据几行几列的位置
        # self.portList.setGeometry(QtCore.QRect(118, 4, 191, 22))
        # 打开/关闭串口 按钮
        self.openCom.clicked.connect(self.port_open_close)        
    #endregion
    #region # 打开/关闭 串口
    def port_open_close(self):
        if (self.openCom.text() == '打开串口') and self.portList.currentText():
            portFullname = self.portList.currentText()                 # 设置端口
            self.ser.port = portFullname[0:portFullname.find(" ")]
            self.ser.baudrate = int(self.sset_cb_baud.currentText())   # 波特率
            self.ser.bytesize = 8                                      # 数据位
            self.ser.parity = 'N'                                      # 校验位
            self.ser.stopbits = 1                                      # 停止位
            if self.ser.baudrate==9600:
                self.ser.timeout = 0.018                          # 超时时间（9600）   接收雷达0.016-0.018  
            elif self.ser.baudrate==115200:
                self.ser.timeout = 0.007                                   # 超时时间（115200）                
                    
            # self.ser.inter_byte_timeout = 0.001                              # 超时时间（9600）   接收雷达0.016-0.018          
            # self.ser.set_buffer_size(rx_size=12800, tx_size=12800)
            
            try:
                self.ser.open()
                # 打开线程接收
                th1 = threading.Thread(target=self.Serial)
                global rxflag
                rxflag=1
                th1.start()
                
            except serial.SerialException:
                QMessageBox.information(None,'Open Port Error', '此串口不能正常打开！')
                return None
            if self.ser.isOpen():
                self.openCom.setText('关闭串口')
                self.openCom.setStyleSheet(
                    '''QPushButton{background:#9AFF9A;border-radius:5px;}''')

        elif self.openCom.text() == '关闭串口':           
            try:
                #关闭所有定时器
                self.timer1_agv_stop.stop() #正在运行中定时器，电机正在运动，等待时间到停止运动    
                # self.timer2_agv_moveCycle.stop()#延时执行定时器，执行开始时就关闭；等待执行时串口关闭需同时关闭       
                self.timer3_camera_query.stop()
                self.timer4_imu_query.stop()                
                self.timer5_agv_angleJudge.stop()#定时执行定时器，满足条件才关闭
                
                self.label_device_state.setText("离线")
                rxflag =0
                self.ser.close()
            except:
                QMessageBox.critical(None, 'Open Port Error', '此串口不能正常关闭！')
                return None
            self.openCom.setText('打开串口')
            self.openCom.setStyleSheet(
                '''QPushButton{background:orange;border-radius:5px;}''')    
    
    #endregion
    
    #region # 连接/断开 服务器 
    def connect_server(self):
        self.signal.print.emit("开始连接")
        if(self.flag_server==0):
            self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 
            try:
                self.s.settimeout(0.3)#设置socket超时时间后，没有接收到数据连接也会断开
                s_ip=self.input_serverIP.text()
                s_port=int(self.input_serverPort.text())
                self.s.connect((s_ip, s_port))
                self.s.settimeout(None)
                self.btn_connect_server.setText("断开服务器")
                self.btn_connect_server.setStyleSheet(
                    '''QPushButton{background:#9AFF9A;border-radius:5px;}''')
                self.flag_server=1
                self.signal.print.emit("服务器已连接")
                self.label_device_state.setText("在线")
                self.th1 = threading.Thread(target=self.server_process)
                self.th1.start()
            except socket.error as e:
                # print("连接错误:", e)
                self.signal.print.emit("连接失败")
                if e.errno == 111:  # Errno 111 对应 "连接被拒绝"
                    print("服务端已关闭")
            except socket.timeout:
                self.signal.show_msg.emit('连接超时！')
                # self.s.close()
            except:
                self.signal.show_msg.emit('连接失败')
                # self.s.close()
        else:
            self.btn_connect_server.setText("连接服务器")
            self.btn_connect_server.setStyleSheet(
                '''QPushButton{background:orange;border-radius:5px;}''')
            self.flag_server = 0
            self.s.close()
            self.signal.print.emit("服务器已断开")
    def fuc_com_server_change(self):
        self.input_serverIP.setText("10.0.0."+self.com_server.currentText())
    def fuc_check_mode_wifiToUart(self):
        if self.check_mode_wifiToUart.isChecked():self.flag_server=0
        else:
            self.flag_server=1
            self.label_device_state.setText("离线")
        self.connect_server()
    #endregion
    #region # 打开/关闭 服务器
    def server_open(self):
        if(self.flag_server==0):
            self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)       
            s_ip=self.input_ip.text()
            s_port=int(self.input_port.text())
            try:
                self.s.bind((s_ip, s_port))
                self.s.listen(5)
                self.btn_server.setText("关闭服务器")
                self.btn_server.setStyleSheet(
                    '''QPushButton{background:#9AFF9A;border-radius:5px;}''')
                self.flag_server=1
                print('Waiting for connection...')
                self.th1 = threading.Thread(target=self.clients_process)
                self.th1.start()
            except:
                self.signal.show_msg.emit('端口已经被占用')
        else:
            self.btn_server.setText("打开服务器")
            self.btn_server.setStyleSheet(
                '''QPushButton{background:orange;border-radius:5px;}''')
            self.flag_server = 0
            self.s.close()
            self.th1.join()
            # stop_thread('testing thread')
            # 关闭所有连接设备
            global client_sockets
            for i in range(0,len(client_sockets)):
                client_sockets[i].close()
            client_sockets=[]
    #endregion
    #region # 服务器连接处理
    def server_process(self):
        while self.flag_server:
            try:
                data = self.s.recv(1024)
                
            except: break
            if not data: break
            # print(data)
            if self.check_mode_wifiToUart.isChecked():
                pattern = rb'(?<!\\);'# 如果';'前为'\'则不进行分割
                datalist = [x for x in re.split(pattern, data) if x]#去掉空''
                for i in datalist:                      
                    pattern = rb'[\\](?=;)'
                    i=re.sub(pattern,b"",i)#如果'\'后面是';'去掉'\'
                    if len(i)>1:
                        self.data_classify(i) 
            else:
                self.server_parse_data(data)
            # self.s.close()
            # print('服务器已断开')
            # self.signal.print.emit('服务器已断开')
    #endregion
    #region # 客户端连接处理
    def clients_process(self):
        def tcplink(sock, addr):
            print('Accept new connection from %s:%s...' % addr)
            self.signal.print.emit('Accept new connection from %s:%s...' % addr)
            while self.flag_server:
                try:
                    data = sock.recv(1024)
                except: break
                if not data: break
                self.server_parse_data(data)
                # self.signal.print.emit('Received: %s' % data)
                # self.signal.print.emit('数据长度为: %d' % len(data))
                # sock.send(data)
            sock.close()
            print('Connection from %s:%s closed.' % addr)
            self.signal.print.emit('Connection from %s:%s closed.' % addr)
        # 处理连接:
        while self.flag_server:
            try:
                sock, addr = self.s.accept()
                device_ip=addr[0]+":"+str(addr[1])
                client_sockets.append(sock)
                print(client_sockets)
                t = threading.Thread(target=tcplink, name='testing thread',args=(sock, addr),daemon=True)
                t.start()               
            except Exception as e:
                pass
    #endregion
    #region # 服务器解析数据
    def server_parse_data(self,data):
        # print(data)
        
        B_TELEGRAM=PGV()
        B_TELEGRAM.Distribution_code=data[12:16]
        if B_TELEGRAM.Distribution_code==b'AFLF':
            CMD_STATE=data[24:64]
            CMD_STATE_note=["未开始"," 开始","执行中"," 错误"," 完成"]
            CMD_STATE_color=["grey"," black","blue"," red"," green"]
            
            for i in range(10):
                self.label_cmdState[i].setText(CMD_STATE_note[CMD_STATE[2+4*i]])
                self.label_cmdState[i].setStyleSheet("color:"+CMD_STATE_color[CMD_STATE[2+4*i]])
                
            flag_cmd_cycle=1    
            for i in range(self.cmd_count):
                if CMD_STATE[2+4*i]!=4:
                    flag_cmd_cycle=0
            if (flag_cmd_cycle==1)&(self.check_cmd_cycle.isChecked()):
                self.fuc_sendCmd_all()
            AGV_STATE=data[64:96]
            # self.signal.print.emit('收到数据: %s' % AGV_STATE)
            self.input_fb_posX.setText(str(int.from_bytes(AGV_STATE[:4], byteorder='little',signed=True)/10))
            self.input_fb_posY.setText(str(int.from_bytes(AGV_STATE[4:8], byteorder='little',signed=True)/10))
            self.input_fb_angle.setText(str(int.from_bytes(AGV_STATE[8:10], byteorder='little',signed=True)/10))
            self.input_fb_speed.setText(str(int.from_bytes(AGV_STATE[12:16], byteorder='little',signed=True)/10))
            # self.input_fb_angle.setText(str(struct.unpack('<H', AGV_STATE[8:10])[0]))
    #endregion
    #region # 向所有客户端发送数据
    def sendData_to_clients(self,S_cmd):
        Block_counter='0001'
        length=str(len(S_cmd)+24).zfill(5)
        # print(length)
        type='N'      
        source='R'    
        target='S'   
        Distribution_code ='ALFF'
        identifier ='FP'
        Function_point ='003631'
        Telegram=Block_counter+length+type+source+target+Distribution_code+identifier+Function_point
        byte_Telegram=Telegram.encode()        
        byte_Telegram+=bytes(S_cmd)
        # byte_Telegram=a.to_bytes(1,byteorder='big')+byte_Telegram
        for i in range(len(client_sockets)):
            client_sockets[i].send(byte_Telegram)
        # print(byte_Telegram)
        # print('报文长度为%d',len(byte_Telegram))
        # print(client_sockets)
    #endregion
    #region # 向服务器（控制板）发送数据
    def sendData_to_server(self,S_cmd):
        Block_counter='0001'
        length=str(len(S_cmd)+24).zfill(5)
        # print(length)
        type='N'      
        source='R'    
        target='S'   
        Distribution_code ='ALFF'
        identifier ='FP'
        Function_point ='003631'
        Telegram=Block_counter+length+type+source+target+Distribution_code+identifier+Function_point
        byte_Telegram=Telegram.encode()        
        byte_Telegram+=bytes(S_cmd)
        self.s.send(byte_Telegram)
    #endregion
    #region # 服务器控制相关
    def query_ip(self):
        # print(MainWindow.sender().text())
        cmd_id=[0x10,0x02]
        cmd_data=[0x00,0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_cmdNumChange(self):
        source = MainWindow.sender()
        index=self.com_num.index(source)
        self.com_name[index].setCurrentIndex(source.currentIndex())#对应的下拉框同步切换
    def fuc_cmdNameChange(self):
        source = MainWindow.sender()
        index=self.com_name.index(source)
        self.com_num[index].setCurrentIndex(source.currentIndex())#对应的下拉框同步切换
    def fuc_cmdSave(self):#批量保存指令
        filename, _ = QFileDialog.getOpenFileName(None, 'Open File', os.getcwd()+"/cmd")
        print(filename)
        with open(filename, "w",encoding='utf-8') as f:
            for i in range(len(self.com_name)):
                f.write(("True" if self.check_cmd[i].isChecked() else "False")+"  "+self.com_name[i].currentText()+"  "+self.com_num[i].currentText()+"  "+self.input_p1[i].text()+"  "+self.input_p2[i].text()+"  "+self.input_p3[i].text()+"  "+"\n")
        self.signal.print.emit("保存完成")
    def fuc_cmdLoad(self):#批量加载指令
        filename, _ = QFileDialog.getOpenFileName(None, 'Open File', os.getcwd()+"/cmd")
        print(filename)
        with open(filename, "r",encoding='utf-8') as f:
            line_number = 0
            for line in f.readlines():
                line=line.strip()
                print(line)
                self.check_cmd[line_number].setChecked(True if line.split("  ")[0] == "True" else False)
                # self.com_name[line_number].setCurrentText(line.split("  ")[1])
                self.com_num[line_number].setCurrentText(line.split("  ")[2])
                self.input_p1[line_number].setText(line.split("  ")[3])
                self.input_p2[line_number].setText(line.split("  ")[4])
                self.input_p3[line_number].setText(line.split("  ")[5])
                line_number += 1
            self.signal.print.emit("加载完成")
        # filepath="log/{0}.txt".format(self.input_record_name.text()) 
        #     with open(filepath, "a+",encoding='utf-8') as f:
        #         f.write("{}\n".format(get_time()+"  "+text))  # 自带文件关闭功能，不需要再写f.close()
        # for i in range(10):
    def fuc_sendCmd(self):#发送单条指令
        for i in range(10):
            if self.check_cmd[i].isChecked():
                num=int(self.com_num[i].currentText(),16)
        source = MainWindow.sender()
        index=self.btn_cmd.index(source)
        num=int(self.com_num[index].currentText(),16)
        num_list=[num&0xff,num>>8]
        name=self.com_name[index].currentText()
        name_list1=[name.encode('utf-8')[0],name.encode('utf-8')[1]]
        # name_list2=[name[0],name[1]]
        p1=int(self.input_p1[index].text())
        p2=int(self.input_p2[index].text())
        p3=int(self.input_p3[index].text())
        if(p1<0):p1=0x100000000+p1
        if(p2<0):p2=0x100000000+p2
        if(p3<0):p3=0x100000000+p3
        p1_list=[p1&0xff,p1>>8&0xff,p1>>16&0xff,p1>>24]
        p2_list=[p2&0xff,p2>>8&0xff,p2>>16&0xff,p2>>24]
        p3_list=[p3&0xff,p3>>8&0xff,p3>>16&0xff,p3>>24]
        # p1_list.reverse()
        # p2_list.reverse()
        # p3_list.reverse()
        S_cmd=num_list+name_list1+p1_list+p2_list+p3_list
        # print(S_cmd)
        # self.sendData_to_clients(S_cmd)
        self.sendData_to_server(S_cmd)        
    def fuc_check_cmd_all(self):#选择所有指令
        if self.check_cmd_all.isChecked():
            for index in range(10):
                self.check_cmd[index].setChecked(True)
        else:
            for index in range(10):
                self.check_cmd[index].setChecked(False)
    def fuc_sendCmd_all(self):#发送多条指令
        S_cmd=[]
        self.cmd_count=0
        for index in range(10):
            if(self.check_cmd[index].isChecked()):
                self.cmd_count+=1
                num=int(self.com_num[index].currentText(),16)
                num_list=[num>>8,num&0xff]
                name=self.com_name[index].currentText()
                name_list1=[name.encode('utf-8')[0],name.encode('utf-8')[1]]
                p1=int(self.input_p1[index].text())
                p2=int(self.input_p2[index].text())
                p3=int(self.input_p3[index].text())
                if(p1<0):p1=0x100000000+p1
                if(p2<0):p2=0x100000000+p2
                if(p3<0):p3=0x100000000+p3
                # p1_list=[p1>>24,p1>>16&0xff,p1>>8&0xff,p1&0xff]# int32_t 单片机内存存储时高位在后
                p1_list=[p1&0xff,p1>>8&0xff,p1>>16&0xff,p1>>24]
                p2_list=[p2&0xff,p2>>8&0xff,p2>>16&0xff,p2>>24]
                p3_list=[p3&0xff,p3>>8&0xff,p3>>16&0xff,p3>>24]
                tem_cmd=num_list+name_list1+p1_list+p2_list+p3_list
                print(tem_cmd)
                S_cmd+=tem_cmd
        # self.sendData_to_clients(S_cmd)
        self.sendData_to_server(S_cmd)
    #endregion
   
    #region # 界面数据更新
    def fuc_update_deviceState(self):
        self.cnt_deviceState+=1
        if(self.cnt_deviceState>3):self.label_device_state.setText("离线");self.cnt_deviceState=0
    def signalCall_update_time(self, text=""):
        self.label_systemTime.setText("系统时间 "+text)
    def signalCall_print(self, text=""):
        self.textBrowser.append(get_time()+"  "+text)   
        self.fuc_savelog(text)
    def signalCall_printSendMessage(self, text=""):
        self.textBrowser.append(get_time()+"  "+"Tx:"+"  "+text)   
        self.fuc_savelog(text)
    def signalCall_printReceiveMessage(self, text=""):
        self.textBrowser.append(get_time()+"  "+"Rx:"+"  "+text)   
        self.fuc_savelog(text)
    def signalCall_show_msg(self, text=""):
        QMessageBox.about(self.note,"note",text) 
    #endregion
    #region # 打开日志文件
    def fuc_openlog(self):
        filename, _ = QFileDialog.getOpenFileName(None, 'Open File', os.getcwd()+"/log")
        if filename:os.startfile(filename)
    def fuc_savelog(self,text):
        if self.check_record.isChecked():
            filepath="log/{0}.txt".format(self.input_record_name.text()) 
            with open(filepath, "a+",encoding='utf-8') as f:
                f.write("{}\n".format(get_time()+"  "+text))  # 自带文件关闭功能，不需要再写f.close()
    #endregion
    #region # IAP远程升级
    def fuc_app_in(self):
        cmd_id=[0x00,0x00]  
        cmd_data=[0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_receive_in(self):
        cmd_id=[0x00,0x00]  
        cmd_data=[0x02]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_iap_in(self):
    #     sendData1=[0x47,0x51,0x55]
    #     sendData2=bytes(sendData1)
    #     self.ser.write(sendData2)
    #     # self.signal.print.emit(HexStrAddSpace(data.hex())) 
    #     if(self.check_showsSendMessage.isChecked()):
    #         self.signalCall_printSendMessage(HexStrAddSpace(sendData2.hex()))  
        cmd_id=[0x00,0x00]  
        cmd_data=[0x01]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_iap_sendBin_2(self):
        self.iap_filename, _ = QFileDialog.getOpenFileName(None, 'Open File', r'E:\program\18stm32f407igt6_rtos_app\MDK-ARM')  
        with open(self.iap_filename, 'rb') as f:
            data = f.read()   
            # data = data[:1024]
            # self.s.send(data)
            buf_size=50*1024
            self.signalCall_print("程序大小为%d个字节"%len(data))  
            sendCount=len(data)//buf_size
            sendLeftByte=len(data)%buf_size
            self.signalCall_print("需要发送%d次"%sendCount) 
            # sendtime=len(data)/31/100
            # self.signalCall_print("所需时间为%ds"%sendtime) 
            for i in range(sendCount):
                sendData1=data[i*buf_size:(i+1)*buf_size]
                sendData2=bytes(sendData1)
                self.s.send(sendData2)
                # if(i%100==0):
                self.signal.print.emit("发送第%d次"%(i+1))
                time.sleep(1.5)
            sendData1=data[sendCount*buf_size:sendCount*buf_size+sendLeftByte]
            sendData2=bytes(sendData1)
            self.s.send(sendData2)
            print('发送完成')
    def fuc_iap_in_2(self):
        print('网口升级')
        self.iap_filename, _ = QFileDialog.getOpenFileName(None, 'Open File', r'E:\program\18stm32f407igt6_rtos_app\MDK-ARM')  
        with open(self.iap_filename, 'rb') as f:
            data = f.read()          
            self.signalCall_print("程序大小为%d个字节"%len(data)) 
        len_data=len(data)-2*50*1024
        print(len_data)
        sendData1=[0x47,0x51,0x01,len_data>>8,len_data&0xff]
        sendData2=bytes(sendData1)
        self.s.send(sendData2)
        # self.signal.print.emit(HexStrAddSpace(data.hex())) 
        if(self.check_showsSendMessage.isChecked()):
            self.signalCall_printSendMessage(HexStrAddSpace(sendData2.hex()))    
    def fuc_iap_complete(self):
        print('完成升级')
        sendData1=[0x47,0x51,0x02]
        sendData2=bytes(sendData1)
        self.s.send(sendData2)
        # self.signal.print.emit(HexStrAddSpace(data.hex())) 
        if(self.check_showsSendMessage.isChecked()):
            self.signalCall_printSendMessage(HexStrAddSpace(sendData2.hex()))    
   
    def fuc_iap_sendBin(self):
        self.iap_filename, _ = QFileDialog.getOpenFileName(None, 'Open File', r'E:\program\27stm32f407igt6_rtos_app1.1 - 8266\MDK-ARM')  
        with open(self.iap_filename, 'rb') as f:
            data = f.read()   
            # data = data[:3*1024]
            # self.ser.write(data)

            self.signalCall_print("程序大小为%d个字节"%len(data))  
            # sendCount=len(data)//31
            # sendLeftByte=len(data)%31
            # self.signalCall_print("需要发送%d次"%sendCount) 
            # sendtime=len(data)/31/100
            # self.signalCall_print("所需时间为%ds"%sendtime) 
            # for i in range(sendCount):
            #     sendData1=data[i*31:(i+1)*31]
            #     sendData2=bytes(sendData1)
            #     self.ser.write(sendData2)
            #     # if(i%100==0):
            #     # self.signal.print.emit("发送第%d次"%(i+1))
            #     # time.sleep(0.001)
            # sendData1=data[sendCount*31:sendCount*31+sendLeftByte]
            sendData2=bytes(data)
            if self.check_mode_wifiToUart.isChecked():
                self.s.send(sendData2)  
            else:
                self.ser.write(sendData2)
            self.signalCall_print('发送完成')



    #endregion
    
    #region # 接收数据
    def Serial(self):
        while rxflag:
            try:
                if self.ser.in_waiting:
                    data= self.ser.readall()#不能接收持续发送的数据
                    if not data or data == 'exit':
                        print("break")
                        break
                    if((data[0] == 0x42)&(data[1] == 0x44)&(data[4]==0x40)&(data[5]==0x00)):#打印完整的回复数据，包括帧头、长度等
                        self.signal.print.emit('Rx:  '+HexStrAddSpace(data[:data[3]].hex()))  
                    # print(data)
                    # datalist = re.split(b";", data)
                    # datalist = [x for x in re.split(b";", data) if x]#去掉空''
                    pattern = rb'(?<!\\);'# 如果';'前为'\'则不进行分割
                    datalist = [x for x in re.split(pattern, data) if x]#去掉空''
                    for i in datalist:    
                        # print(i)                  
                        pattern = rb'[\\](?=;)'
                        i=re.sub(pattern,b"",i)#如果'\'后面是';'去掉'\'
                        if len(i)>1:
                            self.data_classify(i)  
                                      

            except Exception:
                print("wrong")
                # exit()
                self.signal.print.emit("串口异常关闭")
                self.port_open_close()
    
    #endregion
    #region # 接收数据区分开处理
    def data_classify(self,data):
        # print(str(data))
        if(data[0] == 0x50):#接收十六进制数组
            a=HexStrAddSpace(data.hex())
            # self.signal.print.emit(get_time() + "  " + HexStrAddSpace(data.hex()))
        elif((data[0] == 0x42)&(data[1] == 0x44)):#BD（0x42,0x44）来自于控制板的数据
            self.signal.verify_recieveData.emit(data)
            # print("hlly")
        elif((data[0] == 0x00)&(data[1] == 0x44)):#华睿PGV相机原始数据解析
            self.signal.print.emit(HexStrAddSpace(data.hex()))  
            self.signal.verify_recieveData.emit(data)
        elif((data[0] == 0x03)&(data[1] == 0x04)):#倍加福PGV相机原始数据解析
            self.signal.print.emit(HexStrAddSpace(data.hex()))  
            self.signal.verify_recieveData.emit(data)
        else: #接收中文或英文
            # try:a=data.decode('gbk')                              
            try:a=data.decode('utf-8')                              
            except Exception:
                # try:a=data.decode('gbk');print("中文")
                # except : a=str(data)
                a=str(data)
            # self.signal.print.emit(HexStrAddSpace(data.hex()))  
            self.signal.print.emit(a)  
    #endregion
    #region # 接收数据验证
    def signalCall_verify_recieveData(self, data):    
        if((len(data)<100)&(len(data)>data[3])):#多帧数据拆分开处理
            processData=data[:data[3]]
            self.signalCall_verify_recieveData(processData) 
            remainData=data[data[3]:]   
            print("多帧")
            self.signalCall_verify_recieveData(remainData)
        else:
            if(data[len(data)-1]!=0xda):
                # self.signalCall_print("帧尾错误")
                try:print(data);a=data.decode('utf-8');print("english")                        
                except Exception:
                    try:a=data.decode('gbk');print("中文")
                    except : a=str(data)
                # self.signal.print.emit(HexStrAddSpace(data.hex()))  
                self.signal.print.emit(a) 
            else:self.signalCall_process_recieveData(data[4:len(data)-2])          
    #endregion
    #region # 接收数据处理
    def signalCall_process_recieveData(self, data):       
        if((data[0]==0x40)&(data[1]==0x01)):#心跳包
            self.label_device_state.setText("在线")
            self.cnt_deviceState=0
            data=data[2:]          
            agv_currentPosX=listToFloat(data[16:20])
            agv_currentPosY=listToFloat(data[20:24])
            agv_currentAngle=listToFloat(data[24:28])
            agv_ultrasonic=listToFloat(data[28:32])
            self.input_agv_currentPosX.setText(str(agv_currentPosX))#世界坐标X信息
            self.input_agv_currentPosY.setText(str(agv_currentPosY))#世界坐标Y信息
            self.input_agv_curentAngle.setText(str(agv_currentAngle))#角度信息
            self.input_agv_ultrasonic.setText(str(agv_ultrasonic))#角度信息

            #更新运动轨迹
            if((self.last_agv_currentPosX!=agv_currentPosX)|(self.last_agv_currentPosY!=agv_currentPosY)):
                self.paint.dotdata.append({'pos': (agv_currentPosX, agv_currentPosY)})  # 新增点
                self.paint.scatter.setData(self.paint.dotdata)
            self.last_agv_currentPosX=agv_currentPosX
            self.last_agv_currentPosY=agv_currentPosY

            # 更新角度曲线的数据
            self.angleCurve.y[:-1] = self.angleCurve.y[1:]  # 数据向前移动一个位置
            self.angleCurve.y[-1] = agv_currentAngle
            self.angleCurve.plot.setData(self.angleCurve.x, self.angleCurve.y)
            # self.angleCurve.y1[:-1] = self.angleCurve.y1[1:]
            # self.angleCurve.y1[-1] = agv_currentAngle+90
            # self.angleCurve.plot1.setData(self.angleCurve.x, self.angleCurve.y1)
        elif((data[0]==0x40)&(data[1]==0x02)):#控制板数据更新
            data=data[2:]#角度信息
            angle=[data[0],data[1],data[2],data[3]]
            angle=listToFloat(angle)
            self.input_agv_curentAngle.setText(str(angle))
            data=data[4:]#行驶路程信息
            currentDistance=[data[0],data[1],data[2],data[3]]
            currentDistance=listToFloat(currentDistance)
            self.input_agv_driveDistance.setText(str(currentDistance))
            data=data[4:]#世界坐标X信息
            agv_currentPosX=[data[0],data[1],data[2],data[3]]
            agv_currentPosX=listToFloat(agv_currentPosX)
            self.input_agv_currentPosX.setText(str(agv_currentPosX))
            data=data[4:]#世界坐标Y信息
            agv_currentPosY=[data[0],data[1],data[2],data[3]]
            agv_currentPosY=listToFloat(agv_currentPosY)
            self.input_agv_currentPosY.setText(str(agv_currentPosY))
            data=data[4:]#角速度(根据IMU计算)
            agv_imu_rotateSpeed=[data[0],data[1],data[2],data[3]]
            agv_imu_rotateSpeed=listToFloat(agv_imu_rotateSpeed)
            self.input_agv_parameter4_rateSpeed_2.setText(str(agv_imu_rotateSpeed))


            #更新运动轨迹
            if((self.last_agv_currentPosX!=agv_currentPosX)|(self.last_agv_currentPosY!=agv_currentPosY)):
                self.paint.dotdata.append({'pos': (agv_currentPosX, agv_currentPosY),'brush': pg.mkBrush('g'),'pen':pg.mkPen(color='g')})  # 设置每个点的颜色
                self.paint.scatter.setData(self.paint.dotdata)
            self.last_agv_currentPosX=agv_currentPosX
            self.last_agv_currentPosY=agv_currentPosY
        elif((data[0]==0x40)&(data[1]==0x03)):#更新计算目标角度
            data=data[2:]
            angle=[data[0],data[1],data[2],data[3]]
            angle=listToFloat(angle)
            self.input_agv_targetAngle.setText(str(angle))
        
        elif((data[0]==0x40)&(data[1]==0x04)):#更新计算目标距离
            data=data[2:]
            targetDistance=[data[0],data[1],data[2],data[3]]
            targetDistance=listToFloat(targetDistance)
            self.input_agv_targetDistance.setText(str(targetDistance))
        elif((data[0]==0x40)&(data[1]==0x05)):#更新PGV相机信息
            data=data[2:]
            # x
            PGV_posX=data[0]<<24|data[1]<<16|data[2]<<8|data[3]
            if(data[0]>>7):PGV_posX=PGV_posX-0x100000000
            # y
            PGV_posY=data[4]<<8|data[5]
            if(data[4]>>7):PGV_posY=PGV_posY-0x10000
            # w
            PGV_posYaw=data[6]<<8|data[7]
            if(data[6]>>7):PGV_posYaw=PGV_posYaw-0x10000
            PGV_posYaw=round(PGV_posYaw/10,1)
            # number
            PGV_tagNuber=data[8]<<24&0xff|data[9]<<16&0xff|data[10]<<8&0xff|data[11]&0xff

            # 更新PGV数据
            self.input_PGV_posX.setText(str(PGV_posX))
            self.input_PGV_posY.setText(str(PGV_posY))
            self.input_PGV_posYaw.setText(str(PGV_posYaw))
            self.input_PGV_tagNuber.setText(str(PGV_tagNuber))
            # 更新角度曲线的数据
            self.angleCurve.y1[:-1] = self.angleCurve.y1[1:]
            self.angleCurve.y1[-1] = PGV_posYaw
            self.angleCurve.plot1.setData(self.angleCurve.x, self.angleCurve.y1)
        elif((data[0]==0x40)&(data[1]==0x10)):#读取flash参数1-7
            data=data[2:]  
            # print(data)        
            scale_rateToLine=listToFloat(data[0:4])
            agv_default_lineSpeed=listToFloat(data[4:8])
            agv_default_rateSpeed=listToFloat(data[8:12])
            agv_acceleration=listToFloat(data[12:16])
            agv_deceleration=listToFloat(data[16:20])
            agv_RateAcceleration=listToFloat(data[20:24])
            agv_RateDeceleration=listToFloat(data[24:28])
            self.input_scale_rateToLine.setText(str(scale_rateToLine))
            self.input_agv_default_lineSpeed.setText(str(agv_default_lineSpeed))
            self.input_agv_default_rateSpeed.setText(str(agv_default_rateSpeed))
            self.input_agv_acceleration.setText(str(agv_acceleration))
            self.input_agv_deceleration.setText(str(agv_deceleration))
            self.input_agv_RateAcceleration.setText(str(agv_RateAcceleration))
            self.input_agv_RateDeceleration.setText(str(agv_RateDeceleration))
        elif((data[0]==0x40)&(data[1]==0x11)):#读取flash参数8-14
            data=data[2:]  
            # print(data)        
            imu_integralGain=listToFloat(data[0:4])
            imu_GyroOffset_Z=listToFloat(data[4:8])
            agv_angleRangeOffset=listToFloat(data[8:12])
            agv_distanceRangeOffset=listToFloat(data[12:16])
            factor_rectify=listToFloat(data[16:20])
            rectifyAngle=listToFloat(data[20:24])
            self.input_imu_integralGain.setText(str(imu_integralGain))
            self.input_imu_offset.setText(str(imu_GyroOffset_Z))
            self.input_angleRangeOffset.setText(str(agv_angleRangeOffset))
            self.input_distanceRangeOffset.setText(str(agv_distanceRangeOffset))
            self.input_factor_rectify.setText(str(factor_rectify))
            self.input_rectifyAngle.setText(str(rectifyAngle))
            self.btn_read_flashParameter.setText("读取参数")
        elif((data[0]==0x42)&(data[1]==0x00)):# 更新扩展输入口状态
                self.check_input_1.setChecked(True if ((data[2]&0x01)>0) else False)
                self.check_input_2.setChecked(True if ((data[2]&0x02)>0) else False)
                self.check_input_3.setChecked(True if ((data[2]&0x04)>0) else False)
                self.check_input_4.setChecked(True if ((data[2]&0x08)>0) else False)
                self.check_input_5.setChecked(True if ((data[2]&0x10)>0) else False)
                self.check_input_6.setChecked(True if ((data[2]&0x20)>0) else False)
                self.check_input_7.setChecked(True if ((data[2]&0x40)>0) else False)
                self.check_input_8.setChecked(True if ((data[2]&0x80)>0) else False)
        elif((data[0]==0x42)&(data[1]==0x09)):# CAN通信反馈(电机、IMU)
            self.signal.print.emit(HexStrAddSpace(data[2:].hex()))  
            canId=data[2]<<8|data[3]
            canData=data[4:]
            if(canId==0x700+int(self.input_motor_nodeId.text())):               
                if(canData[0]==0x00):self.label_motor_heartState.setText("启动状态")
                if(canData[0]==0x02):self.label_motor_heartState.setText("连接状态")
                if(canData[0]==0x04):self.label_motor_heartState.setText("停止状态")
                if(canData[0]==0x05):self.label_motor_heartState.setText("操作状态")
                if(canData[0]==0x7F):
                    self.label_motor_heartState.setText("预操作状态")
                    self.label_motor_active.setText("已激活")
            # if(canId==0x180+int(self.input_motor_nodeId.text())): 
            #     speed=canData[1]<<8|canData[0]
            #     if(canData[1]>>7):speed-=0x10000
            #     self.label_motor_speed.setText(str(int(speed))+'rpm')
            if(canId==0x585):# 陀螺仪数据
                if(canData[3]==0X01):
                    angle=[canData[4],canData[5],canData[6],canData[7]]
                    angle=listToFloat(angle)
                    self.input_agv_curentAngle.setText(str(angle))
                if(canData[3]==0X02):
                    anglerRate=canData[4]+canData[5]<<8+canData[6]<<16+canData[7]<<24
                    
                    print(anglerRate)
                    # self.input_imu_angleRate.setText(str(anglerRate)+'rad/s')


        if((data[0]==0x42)&(data[1]==0x20)):#处理读码相机数据
            self.signalCall_process_PGVData(data[2:])
    #endregion
    #region # 处理PGV相机数据
    def signalCall_process_PGVData(self, data): 
        self.signalCall_printReceiveMessage(HexStrAddSpace(data.hex()))
        self.pgv.ERR		=	data[0]&0x01
        self.pgv.NP			=	data[0]&0x02
        self.pgv.WRN		=	data[0]&0x04
        self.pgv.CC1		=	data[0]&0x08
        self.pgv.A0			=	data[0]&0x10
        self.pgv.A1			=	data[0]&0x20
        self.pgv.CC2		=	data[0]&0x40
        self.pgv.reserve01	=	data[0]&0x80            
        self.pgv.RL			=   data[1]&0x01
        self.pgv.LL			=   data[1]&0x02
        self.pgv.NL			=   data[1]&0x04
        self.pgv.RP			=   data[1]&0x08
        self.pgv.LC0		=   data[1]&0x10
        self.pgv.LC1		=   data[1]&0x20
        self.pgv.TA			=   data[1]&0x40
        self.pgv.reserve02	=   data[1]&0x80  
        #posX
        if(data[2]>>2):#/*true 表示posX是负数，false 表示posX是正数*/

            tmp_posX = (data[5]&0x7f)|((data[4]&0x7f)<<7)|((data[3]&0x7f)<<14)|((data[2]&0x7)<<21)
            tmp_posX = ((~tmp_posX+0x01)&0xffffff)*-1.0;	
        else:
            tmp_posX = (data[5]&0x7f)|((data[4]&0x7f)<<7)|((data[3]&0x7f)<<14)|((data[2]&0x7)<<21)
            print('hlly')
            print(data[5])
            print(data[4])
            print(data[3])
            print(data[2])

        # io_Data->posX = i_Para.resolutionX * (tmp_posX) * 1.0;

        #posY
        if(data[6]>>6):#/*true 表示posY是负数，false 表示posY是正数*/

            tmp_posY = (data[7]&0x7f)|((data[6]&0x7f)<<7)
            tmp_posY = ((~tmp_posY+0x01)&0x3fff)*-1.0;	
        else:
            tmp_posY = (data[7]&0x7f)|((data[6]&0x7f)<<7)

        # io_Data->posY = i_Para.resolutionY * (tmp_posY) * 1.0;

        #posYAW
        if(data[10]>>6):#/*true 表示posYAW是负数，false 表示posYAW是正数*/
            tmp_posYaw = (data[11]&0x7f)|((data[10]&0x7f)<<7)
            tmp_posYaw = ((~tmp_posYaw+0x01)&0x3fff)*-1.0;	
        else:
            tmp_posYaw = (data[11]&0x7f)|((data[10]&0x7f)<<7)
        print("X位置%d,Y位置%d,角度%d"%(tmp_posX,tmp_posY,tmp_posYaw//10))
        self.label_pgv_posX.setText(str(tmp_posX)+'mm')
        self.label_pgv_posY.setText(str(tmp_posY)+'mm')
        self.label_pgv_angle.setText(str(tmp_posYaw//10)+'°')
    #endregion
    #region # 接收数据清空
    def fuc_clear(self, text=""):
        self.textBrowser.clear()


    #endregion
    
    #region # 发送数据
    def fuc_serial_send(self,cmd_id,cmd_data):
        cmd_header = [0x50, 0x43]
        cmd_length = [0x00, 0x00]
        cmd_check = [0x00]
        cmd_end = [0xda]
        cmd_length[1]=len(cmd_data)+8
        sendData0=cmd_header+cmd_length+cmd_id+cmd_data
        cmd_check[0]=SumCheck(sendData0,cmd_length[1]-2)
        sendData1=sendData0+cmd_check+cmd_end
        sendData2=bytes(sendData1)
        if self.check_mode_wifiToUart.isChecked():
            self.s.send(sendData2)  
        else:
            self.ser.write(sendData2)
        # self.signal.print.emit(HexStrAddSpace(data.hex())) 
        if(self.check_showsSendMessage.isChecked()):
            self.signalCall_printSendMessage(HexStrAddSpace(sendData2.hex()))
    #endregion
    #region # 控制语音播报
    def fuc_updateAudio(self):
        cmd_id=[0x40,0x02]
        flag_updateAudio=self.check_updateAudio.isChecked()
        cmd_data=[flag_updateAudio]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_playVoice(self):
        cmd_id=[0x40,0x03]
        # voiceNumber=self.com_voiceList.currentIndex()#0x00-0xff播放第几条语音(1到256条)
        voiceNumber=int(self.com_voiceList.currentText().split('.')[0])
        volume=self.slider_volume.value()
        cycleTime=int(self.input_cycleTime.text())#0x01-0xfe循环次数(1到255次)，0x00 停止播报，0xff无限循环播报
        cmd_data=[voiceNumber,cycleTime,volume]
        self.fuc_serial_send(cmd_id,cmd_data)
        message = "播放语音%s，循环次数%d"%(self.com_voiceList.currentText(),cycleTime)
        self.signalCall_print(message)
    def fuc_pauseVoice(self):
        cmd_id=[0x40,0x03]
        voiceNumber=0x00
        volume=self.slider_volume.value()
        cycleTime=int(self.input_cycleTime.text())
        cmd_data=[voiceNumber,cycleTime,volume]
        self.fuc_serial_send(cmd_id,cmd_data)
        message = "停止播放"
        self.signalCall_print(message)    
    def fuc_changeVolume(self):
        self.label_volume.setText(str(self.slider_volume.value()))
    #endregion
    #region # 控制LED
    def fuc_ledSwitch(self):
        cmd_id=[0x40,0x04]
        ledNumber=self.com_ledNumber.currentIndex()+1
        ledState=self.check_ledSwitch.isChecked()
        cmd_data=[ledNumber,ledState]
        self.fuc_serial_send(cmd_id,cmd_data)
        message1="打开" if ledState==True else "关闭"
        message =message1+ "%d号灯"%(ledNumber)
        self.signalCall_print(message)
    def fuc_ledBlink(self):
        cmd_id=[0x40,0x05]
        ledNumber=self.com_ledNumber.currentIndex()+1
        blinkInterval=int(self.input_blinkInterval.text())
        blinkCnt=int(self.input_blinkCnt.text())
        cmd_data=[ledNumber,blinkInterval,blinkCnt]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_ledsSwitch(self):
        cmd_id=[0x40,0x06]
        ledNumber=self.check_led_8.isChecked()<<7|self.check_led_7.isChecked()<<6|self.check_led_6.isChecked()<<5|self.check_led_5.isChecked()<<4|\
            self.check_led_4.isChecked()<<3|self.check_led_3.isChecked()<<2|self.check_led_2.isChecked()<<1|self.check_led_1.isChecked()<<0
        ledState=self.check_ledsSwitch.isChecked()
        cmd_data=[ledNumber,ledState]
        self.fuc_serial_send(cmd_id,cmd_data)
        message="打开" if ledState==True else "关闭"
        if(self.check_led_1.isChecked()):message+=" 1号灯,"
        if(self.check_led_2.isChecked()):message+=" 2号灯,"
        if(self.check_led_3.isChecked()):message+=" 3号灯,"
        if(self.check_led_4.isChecked()):message+=" 4号灯"
        self.signalCall_print(message)
    def fuc_ledsBlink(self):
        cmd_id=[0x40,0x07]
        ledNumber=self.check_led_8.isChecked()<<7|self.check_led_7.isChecked()<<6|self.check_led_6.isChecked()<<5|self.check_led_5.isChecked()<<4|\
            self.check_led_4.isChecked()<<3|self.check_led_3.isChecked()<<2|self.check_led_2.isChecked()<<1|self.check_led_1.isChecked()<<0
        blinkInterval=int(self.input_blinkInterval.text())
        blinkCnt=int(self.input_blinkCnt.text())
        cmd_data=[ledNumber,blinkInterval,blinkCnt]
        self.fuc_serial_send(cmd_id,cmd_data)
    #endregion
    #region # 控制IO扩展模块
    def fuc_outputControl(self,outputState):
        cmd_id=[0x40,0x08]
        outputNumber=self.check_output_1.isChecked()<<0|self.check_output_2.isChecked()<<1|self.check_output_3.isChecked()<<2|self.check_output_4.isChecked()<<3|\
                    self.check_output_5.isChecked()<<4|self.check_output_6.isChecked()<<5|self.check_output_7.isChecked()<<6|self.check_output_8.isChecked()<<7
        outputState=self.check_ledSwitch.isChecked()
        cmd_data=[outputNumber,outputState]
        self.fuc_serial_send(cmd_id,cmd_data)
        # message1="打开" if ledState==True else "关闭"
        # message =message1+ "%d号灯"%(ledNumber)
        # self.signalCall_print(message)
    #endregion
    #region # 控制电机
    def fuc_2motor_setSpeed1(self,trig):
        cmd_id=[0x40,0x09] 
        CanId=[0x06,int(self.input_2motor_nodeId_1.text())]
        #速度改变时，滑动条和文本框联动
        if(trig==1):self.input_2motor_speed_1.setText(str(self.slider_2motor_setSpeed_1.value()))
        elif(trig==0):self.slider_2motor_setSpeed_1.setValue(int(self.input_2motor_speed_1.text()))
        
        if(trig==2):speed=0#停止按钮触发
        else:
            if(self.check_2motor_type.isChecked()):speed=int(self.input_2motor_speed_1.text())*10#电机厂家为天太，速度换算公式不一样
            else:speed=int(self.input_2motor_speed_1.text())*2730#DEC=[(RPM*512*编码器分辨率)/1875]（编码器分辨率 10000）
            if speed<0:speed=0x100000000-(-speed)  
        CanData=[0x23,0xFF,0x60,0x00,speed&0x000000ff,speed>>8&0x0000ff,speed>>16&0x00ff,speed>>24&0xff]
        cmd_data=CanId+CanData
        self.fuc_serial_send(cmd_id,cmd_data) 
    def fuc_2motor_setSpeed2(self,trig):
        cmd_id=[0x40,0x09] 
        CanId=[0x06,int(self.input_2motor_nodeId_2.text())]
        #速度改变时，滑动条和文本框联动
        if(trig==1):self.input_2motor_speed_2.setText(str(self.slider_2motor_setSpeed_2.value()))
        elif(trig==0):self.slider_2motor_setSpeed_2.setValue(int(self.input_2motor_speed_2.text()))
        
        if(trig==2):speed=0#停止按钮触发
        else:
            if(self.check_2motor_type.isChecked()):speed=int(self.input_2motor_speed_2.text())*10#电机厂家为天太，速度换算公式不一样
            else:speed=int(self.input_2motor_speed_2.text())*2730#DEC=[(RPM*512*编码器分辨率)/1875]（编码器分辨率 10000）
            if speed<0:speed=0x100000000-(-speed)  
        CanData=[0x23,0xFF,0x60,0x00,speed&0x000000ff,speed>>8&0x0000ff,speed>>16&0x00ff,speed>>24&0xff]
        cmd_data=CanId+CanData
        self.fuc_serial_send(cmd_id,cmd_data) 
    def fuc_2motor_setSpeed(self,trig):
        self.slider_2motor_setSpeed_1.setValue(int(self.input_2motor_speed_1.text()))
        self.slider_2motor_setSpeed_2.setValue(int(self.input_2motor_speed_2.text()))
        cmd_id=[0x40,0x10]
        CanId1=[0x06,int(self.input_2motor_nodeId_1.text())]
        CanId2=[0x06,int(self.input_2motor_nodeId_2.text())]
        if(trig==0):speed1=0;speed2=0 #停止按钮按下
        else:
            if(self.check_2motor_type.isChecked()):speed1=int(self.input_2motor_speed_1.text())*10#电机厂家为天太，速度换算公式不一样
            else:speed1=int(self.input_2motor_speed_1.text())*2730#DEC=[(RPM*512*编码器分辨率)/1875]（编码器分辨率 10000）
            if speed1<0:speed1=0x100000000-(-speed1)  
            if(self.check_2motor_type.isChecked()):speed2=int(self.input_2motor_speed_2.text())*10
            else:speed2=int(self.input_2motor_speed_2.text())*2730#DEC=[(RPM*512*编码器分辨率)/1875]（编码器分辨率 10000）
            if speed2<0:speed2=0x100000000-(-speed2) 
        speedData1=[speed1&0x000000ff,speed1>>8&0x0000ff,speed1>>16&0x00ff,speed1>>24&0xff]
        speedData2=[speed2&0x000000ff,speed2>>8&0x0000ff,speed2>>16&0x00ff,speed2>>24&0xff]
        cmd_data=CanId1+speedData1+CanId2+speedData2
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_motor_setSpeed(self,trig):
        cmd_id=[0x60,0xFF]
        if(trig==1):self.input_motor_speed.setText(str(self.slider_motor_setSpeed.value()))#文本框触发速度
        elif(trig==0):self.slider_motor_setSpeed.setValue(int(self.input_motor_speed.text()))#滑动条触发速度
        if(trig==2):speed=0#停止按钮触发
        else:
            speed=int(self.input_motor_speed.text())*10
            if speed<0:speed=0x10000-(-speed)
        cmd_data=[speed%256,speed//256]
        self.fuc_serial_send(cmd_id,cmd_data)
    #endregion
    #region # 配置电机
    def fuc_motor_configAll(self,CanId):#配置 单个电机 多个参数   
        self.fuc_motor_config("active",CanId)
        time.sleep(3)
        self.fuc_motor_config("setMode",CanId)
        time.sleep(0.03)
        self.fuc_motor_config("clearError",CanId)
        time.sleep(0.03)
        self.fuc_motor_config("prepareServe",CanId)
        time.sleep(0.03)
        self.fuc_motor_config("waitEn",CanId)
        time.sleep(0.03)
        self.fuc_motor_config("en",CanId)
        time.sleep(0.03)
        self.fuc_motor_config("setSpeedUpTime",CanId)
        time.sleep(0.03)
        self.fuc_motor_config("setSpeedDownTime",CanId)   
        time.sleep(0.03) 
    def fuc_motors_configAll(self):#配置 多个电机 多个参数   
        if(self.check_motor_nodeId.isChecked()):
            CanId=[0x06,int(self.input_motor_nodeId.text())]  
            self.fuc_motor_configAll(CanId)
        if(self.check_motor_nodeId_2.isChecked()):
            CanId=[0x06,int(self.input_motor_nodeId_2.text())]  
            self.fuc_motor_configAll(CanId)
    def fuc_motor_preConfigPDO(self,note,PDOid):
        cmd_id=[0x40,0x09]
        CanId=[0x06,int(self.input_motor_nodeId.text())] 
        if(note=="PDO_forbid"):#1.禁止PDO 
            CanData=[0x23,PDOid,0x18,0x01,0x82,0x01,0x00,0x80]
        if(note=="PDO_mode"):#2.配置TPDO1为异步
            CanData=[0x2F,PDOid,0x18,0x02,0xFF,0x00,0x00,0x00]
        if(note=="PDO_forbidTime"):#3.设置异步模式下的禁止时间
            CanData=[0x2F,PDOid,0x18,0x03,0x10,0x27,0x00,0x00]
        if(note=="PDO_time"):#4.设置异步模式下TPDO1的发送数据时间间隔
            if(PDOid==0):time_interval=int(self.input_motor_timeInterval.text())
            if(PDOid==1):time_interval=int(self.input_motor_timeInterval_2.text())
            if(PDOid==2):time_interval=int(self.input_motor_timeInterval_3.text())
            if(PDOid==3):time_interval=int(self.input_motor_timeInterval_4.text())
            CanData=[0x2F,PDOid,0x18,0x05,time_interval&0xff,time_interval>>8,0x00,0x00]
        if(note=="PDO_clear"):#5.清空1A00的映射PDO个数
            CanData=[0x2F,PDOid,0x1A,0x00,0x00,0x00,0x00,0x00]
        if(note=="PDO_address_1"):#6.把3000映射到”1A00：01”
            # if(PDOid==0):
            #     address=int(self.com_motor_map1_1.currentText(),16)
            #     indexLenth=self.canOpenDic[self.com_motor_map1_1.currentText()][1]#索引长度代码
            #     lenth=self.indexLenth[indexLenth]#映射字节长度
            #     self.mapNumber+=1
            # CanData=[0x23,PDOid,0x1A,0x01,indexLenth,0x00,address&0xff,address>>8]
            CanData=[0x23,0x00,0x1A,0x01,0x10,0x00,0x00,0x30]
        if(note=="PDO_address_2"):#6.把3000映射到”1A00：01”
            CanData=[0x23,0x00,0x1A,0x02,0x10,0x00,0x04,0x30]
        if(note=="PDO_address_3"):#6.把3000映射到”1A00：01”
            CanData=[0x23,0x00,0x1A,0x03,0x20,0x00,0x01,0x30]
        if(note=="PDO_numer"):#7.设置1A00的映射PDO个数
            # CanData=[0x2F,PDOid,0x1A,0x00,self.mapNumber,0x00,0x00,0x00]
            CanData=[0x2F,PDOid,0x1A,0x00,0x03,0x00,0x00,0x00]
            self.mapNumber=0
        if(note=="PDO_enable"):#8.使能PDO 
            CanData=[0x23,PDOid,0x18,0x01,0x82,0x01,0x00,0x00]
        cmd_data=CanId+CanData
        self.fuc_serial_send(cmd_id,cmd_data)    
    def fuc_motors_configPDO(self,PDOid):  
        self.fuc_motor_preConfigPDO("PDO_forbid",PDOid)
        time.sleep(0.03)
        self.fuc_motor_preConfigPDO("PDO_mode",PDOid)
        time.sleep(0.03)
        self.fuc_motor_preConfigPDO("PDO_forbidTime",PDOid)
        time.sleep(0.03)
        self.fuc_motor_preConfigPDO("PDO_time",PDOid)
        time.sleep(0.03)
        self.fuc_motor_preConfigPDO("PDO_clear",PDOid)
        time.sleep(0.03)
        self.fuc_motor_preConfigPDO("PDO_address_1",PDOid)
        time.sleep(0.03)
        self.fuc_motor_preConfigPDO("PDO_address_2",PDOid)
        time.sleep(0.03)
        self.fuc_motor_preConfigPDO("PDO_address_3",PDOid)
        time.sleep(0.03)
        self.fuc_motor_preConfigPDO("PDO_numer",PDOid)
        time.sleep(0.03)
        self.fuc_motor_preConfigPDO("PDO_enable",PDOid)    
    def fuc_motor_preConfig(self,note):#预配置   
        if(note=="waitEn"):
            if(self.check_motor_en.isChecked()):self.check_motor_en.setChecked(False)
            else:self.fuc_motors_config("waitEn")
        if(note=="en"):
            if(self.check_motor_en.isChecked()):self.fuc_motors_config("en")
            else:self.check_motor_en.setChecked(True)    
    def fuc_motors_config(self,note):#配置 多个电机 单个参数   
        if(self.check_motor_nodeId.isChecked()):
            CanId=[0x06,int(self.input_motor_nodeId.text())]  
            self.fuc_motor_config(note,CanId)
        if(self.check_motor_nodeId_2.isChecked()):
            CanId=[0x06,int(self.input_motor_nodeId_2.text())]  
            self.fuc_motor_config(note,CanId)
    def fuc_motor_config(self,note,CanId):#配置 单个电机 单个参数       
        cmd_id=[0x40,0x09]        
        if(note=="active"):CanId[0]=0x00 
        else:CanId[0]=0x06
        if(note=="active"):
            self.label_motor_active.setText("激活中")
            CanData=[0x10,0x10]
        if(note=="setMode"):#步科电机连续运行模式为-3（0xFD）
            CanData=[0x2F,0x60,0x60,0x00,0xFD,0x00,0x00,0x00]
        if(note=="clearError"):
            CanData=[0x2B,0x40,0x60,0x00,0x80,0x00,0x00,0x00]
        if(note=="prepareServe"):
            CanData=[0x2B,0x40,0x60,0x00,0x06,0x00,0x00,0x00]       
        if(note=="waitEn"):
            CanData=[0x2B,0x40,0x60,0x00,0x07,0x00,0x00,0x00]
        if(note=="en"):
            CanData=[0x2B,0x40,0x60,0x00,0x0F,0x00,0x00,0x00]
        if(note=="check_en"):
            if(self.check_motor_en.isChecked()):CanData=[0x2B,0x40,0x60,0x00,0x0F,0x00,0x00,0x00]
            else:CanData=[0x2B,0x40,0x60,0x00,0x07,0x00,0x00,0x00]
        if(note=="setSpeedUpTime"):#加减速时间
            CanData=[0x23,0x1C,0x20,0x00,0xB8,0x0B,0x00,0x00]
            CanData[4]=int(self.input_time_speedUp.text())%256
            CanData[5]=int(self.input_time_speedUp.text())//256
        if(note=="setSpeedDownTime"):
            CanData=[0x23,0x1D,0x20,0x00,0xB8,0x0B,0x00,0x00]
            CanData[4]=int(self.input_time_speedDown.text())%256
            CanData[5]=int(self.input_time_speedDown.text())//256
        if(note=="NMT"):# NMT（Network Management 网络管理）
            CanId=[0x00,0x00]
            NMT=[0x01,0x02,0x80,0x81,0x82] 
            CanData=[NMT[self.com_motor_NMT.currentIndex()],int(self.input_motor_nodeId.text())]
            print('hlly')
        cmd_data=CanId+CanData
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_motor_en(self,CanId):
        cmd_id=[0x40,0x11]  
        cmd_data=[CanId,0x00]  
        if(CanId==2):cmd_data[1]=self.check_2motor_en_1.isChecked()
        if(CanId==3):cmd_data[1]=self.check_2motor_en_2.isChecked()
        self.fuc_serial_send(cmd_id,cmd_data)
    #endregion
    #region # 控制AGV
    
    def fuc_agv_moveForward(self):#前进
        cmd_id=[0x40,0x60]
        lineSpeed=float(self.input_agv_default_lineSpeed.text())
        rateSpeed=0
        cmd_data=[0x1F,]+floatToList(lineSpeed)+floatToList(rateSpeed)       
        self.fuc_serial_send(cmd_id,cmd_data)
        self.add_keyDot(lineSpeed,rateSpeed)
    def fuc_agv_moveBack(self):#后退
        cmd_id=[0x40,0x60]
        lineSpeed=-float(self.input_agv_default_lineSpeed.text())
        rateSpeed=0
        cmd_data=[0x1F,]+floatToList(lineSpeed)+floatToList(rateSpeed)       
        self.fuc_serial_send(cmd_id,cmd_data)
        self.add_keyDot(lineSpeed,rateSpeed)
    def fuc_agv_rotateCW(self):#正转
        cmd_id=[0x40,0x60]
        lineSpeed=0
        rateSpeed=-float(self.input_agv_default_rateSpeed.text())
        cmd_data=[0x1F,]+floatToList(lineSpeed)+floatToList(rateSpeed)       
        self.fuc_serial_send(cmd_id,cmd_data)
        self.add_keyDot(lineSpeed,rateSpeed)
    def fuc_agv_rotateCCW(self):#反转
        cmd_id=[0x40,0x60]
        lineSpeed=0
        rateSpeed=float(self.input_agv_default_rateSpeed.text())
        cmd_data=[0x1F,]+floatToList(lineSpeed)+floatToList(rateSpeed)       
        self.fuc_serial_send(cmd_id,cmd_data)
        self.add_keyDot(lineSpeed,rateSpeed)
    """
    def fuc_agv_moveForward(self):#前进
        cmd_id=[0x40,0x31]
        cmd_data=[0,0]   
        self.fuc_serial_send(cmd_id,cmd_data)        
    def fuc_agv_moveBack(self):#后退
        cmd_id=[0x40,0x60]
        lineSpeed=floatToList(-float(self.input_agv_default_lineSpeed.text()))
        rateSpeed=floatToList(0)
        cmd_data=[0x1F,]+floatToList(lineSpeed)+floatToList(rateSpeed)       
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_agv_rotateCW(self):#正转
        cmd_id=[0x40,0x60]
        lineSpeed=floatToList(0)
        rateSpeed=floatToList(-float(self.input_agv_default_rateSpeed.text()))
        cmd_data=[0x1F,]+floatToList(lineSpeed)+floatToList(rateSpeed)       
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_agv_rotateCCW(self):#反转
        cmd_id=[0x40,0x60]
        lineSpeed=floatToList(0)
        rateSpeed=floatToList(float(self.input_agv_default_rateSpeed.text()))
        cmd_data=[0x1F,]+floatToList(lineSpeed)+floatToList(rateSpeed)       
        self.fuc_serial_send(cmd_id,cmd_data)
    """
    def fuc_agv_run(self):#曲线运行
        cmd_id=[0x40,0x60]
        lineSpeed=float(self.input_agv_lineSpeed.text())
        rateSpeed=float(self.input_agv_rateSpeed.text())
        cmd_data=[0x1F,]+floatToList(lineSpeed)+floatToList(rateSpeed)       
        self.fuc_serial_send(cmd_id,cmd_data)
        self.add_keyDot(lineSpeed,rateSpeed)

    def fuc_agv_runBack(self):#曲线运行
        cmd_id=[0x40,0x60]
        lineSpeed=-float(self.input_agv_lineSpeed.text())
        rateSpeed=-float(self.input_agv_rateSpeed.text())
        cmd_data=[0x1F,]+floatToList(lineSpeed)+floatToList(rateSpeed)       
        self.fuc_serial_send(cmd_id,cmd_data)
        self.add_keyDot(lineSpeed,rateSpeed)
    def fuc_agv_stop(self):#停止
        cmd_id=[0x40,0x60]
        lineSpeed=0
        rateSpeed=0
        cmd_data=[0x1F,]+floatToList(lineSpeed)+floatToList(rateSpeed)       
        self.fuc_serial_send(cmd_id,cmd_data)
        self.add_keyDot(lineSpeed,rateSpeed)
    def fuc_agv_toAngle(self): 
        if(self.check_mode_retransfer.isChecked()):
            angleDeviation=float(self.input_agv_targetAngle.text())-float(self.input_agv_curentAngle.text())#目标角度-当前角度
            if(abs(angleDeviation)>float(self.input_imu_angleDeviation.text())):#判断是否需要旋转
                self.timer5_agv_angleJudge.start(10)
                if(((angleDeviation>0)&(angleDeviation<180))|(angleDeviation<-180)):self.fuc_agv_rotateCCW();print('逆时针')#判断旋转方向
                else:self.fuc_agv_rotateCW();print('顺时针')

        else:#控制板控制 
            cmd_id=[0x40,0x32]
            angle=int(float(self.input_agv_targetAngle.text())*10)
            cmd_data=[angle>>8&0xff,angle&0xff]
            self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_agv_angleJudge(self): #判断是否旋转到位
        # print('hlly')
        if(self.check_agv_anologAngle.isChecked()):#模拟旋转
            rate=30 if self.agv_rotateDirection=="CCW" else -30
            dw=rate*0.01
            angle=round(float(self.input_agv_curentAngle.text())+dw,1)
            self.input_agv_curentAngle.setText(str(angle))
        currentAngle=float(self.input_agv_curentAngle.text())
        if(currentAngle<-180):currentAngle+=360;self.input_agv_curentAngle.setText(str(currentAngle))
        elif(currentAngle>180):currentAngle-=360;self.input_agv_curentAngle.setText(str(currentAngle))
        angleDeviation=float(self.input_agv_targetAngle.text())-float(self.input_agv_curentAngle.text())
        if(abs(angleDeviation)<float(self.input_imu_angleDeviation.text())):            
            self.fuc_agv_autoStop()
    def fuc_agv_toDistance(self):
        
        if(self.check_mode_retransfer.isChecked()):
            #更新目标位置
            s=float(self.input_agv_targetDistance.text())   
            # self.x2= round(s*math.cos(float(self.input_agv_curentAngle.text())*math.pi/180),2)
            # self.y2= round(s*math.sin(float(self.input_agv_curentAngle.text())*math.pi/180),2)
            # self.input_agv_targetPosX.setText(str(self.x2))
            # self.input_agv_targetPosY.setText(str(self.y2))

            self.timer7_agv_toDistance.stop()
            self.fuc_agv_speedChange(0)
            speed=abs(int(self.input_2motor_speed_1.text()))
            if(s<0):speed=-speed       
            self.input_2motor_speed_1.setText(str(speed))
            self.input_2motor_speed_2.setText(str(speed))
            self.fuc_2motor_setSpeed(1)

            self.input_agv_driveDistance.setText(str(0))#运行前里程计清零
            self.timer6_agv_odometer.start(10)
        else:#控制板控制 
            cmd_id=[0x40,0x33]
            # distance=int(self.input_agv_targetDistance.text())
            cmd_data=floatToList(round(float(self.input_agv_targetDistance.text()) ,2))
            # cmd_data=[distance>>8&0xff,distance&0xff]
            self.fuc_serial_send(cmd_id,cmd_data)
    
    def fuc_agv_odometer(self):#里程计
        s=float(self.input_agv_driveDistance.text())
        ds=int(self.input_agv_parameter3_realSpeed.text())*0.01*0.1
        s+=ds
        s=round(s,2)
        self.input_agv_driveDistance.setText(str(s))
        if(abs(s)>(abs(float(self.input_agv_targetDistance.text()))-float(self.input_agv_distanceDeviation.text()))):#行驶路程超过设定路程
            self.fuc_agv_autoStop()
        #更新当前X坐标
        posX=float(self.input_agv_currentPosX.text())
        dx=ds*math.cos(float(self.input_agv_curentAngle.text())*math.pi/180)#注：钝角倒退时dx为正
        posX+=dx
        posX=round(posX,2)
        self.input_agv_currentPosX.setText(str(posX))
        #更新当前Y坐标
        posY=float(self.input_agv_currentPosY.text())
        dy=ds*math.sin(float(self.input_agv_curentAngle.text())*math.pi/180)#注：钝角倒退时dY为正
        posY+=dy
        posY=round(posY,2)
        self.input_agv_currentPosY.setText(str(posY))
        
        #地图更新轨迹
        # self.paint.plot.clear()
        # self.paint.plot.plot(pen='b',symbol='o').setData([0,posX],[0,posY])
        # # self.paint.plot.plot(pen='b',symbol='o').setData(self.paint.x,self.paint.data)
        # self.paint.plot.update()
    def fuc_agv_toPos(self):
        if(self.check_mode_retransfer.isChecked()):
            # self.input_agv_targetDistance.setText(str(0))
            x1=round(float(self.input_agv_currentPosX.text()),2)
            y1=round(float(self.input_agv_currentPosY.text()),2)
            self.x2= round(float(self.input_agv_targetPosX.text()) ,2)
            self.y2= round(float(self.input_agv_targetPosY.text()) ,2)
            # if(abs(x2-x1)<0.1): #X 坐标相等时，及斜率为无穷大，k=(y2-y1)/(x2-x1)
            #     if(abs(y2-y1)>0.1):
            #         if(y2>y1):self.input_agv_targetAngle.setText('90')
            #         else:self.input_agv_targetAngle.setText('-90')
            #         self.fuc_agv_toAngle()
            #     else:print("目标点离当前距离太近")
            # else:
            #     # k=(y2-y1)/(x2-x1)
            #atan优化为atan2
            if((abs(self.y2-y1)+abs(self.x2-x1))>0.5):
                angle=math.atan2(self.y2-y1,self.x2-x1)/math.pi*180
                angle=round(angle,1)
                self.input_agv_targetAngle.setText(str(angle))#需要旋转多少角度
                self.agv_moveState="toPos1_rotate"
                self.fuc_agv_toAngle()


                distance=math.sqrt((self.x2-x1)*(self.x2-x1)+(self.y2-y1)*(self.y2-y1))
                distance=round(distance,1)
                self.input_agv_targetDistance.setText(str(distance))#需要行进多少距离
            else:print("目标点离当前距离太近")
        else:#控制板控制 
            cmd_id=[0x40,0x34]
            agv_targetPosX= floatToList(round(float(self.input_agv_targetPosX.text()) ,2))
            agv_targetPosY= floatToList(round(float(self.input_agv_targetPosY.text()) ,2))
            cmd_data=agv_targetPosX+agv_targetPosY
            self.fuc_serial_send(cmd_id,cmd_data)
        #清空运动轨迹
        # self.paint.dotdata=[]
        self.paint.dotdata.append({'pos': (round(float(self.input_agv_targetPosX.text()) ,2), round(float(self.input_agv_targetPosY.text()) ,2)),'brush': pg.mkBrush('r')})  # 设置每个点的颜色
        self.paint.scatter.setData(self.paint.dotdata)
        x1=round(float(self.input_agv_currentPosX.text()) ,2)
        y1=round(float(self.input_agv_currentPosY.text()) ,2)
        x2=round(float(self.input_agv_targetPosX.text()) ,2)
        y2=round(float(self.input_agv_targetPosY.text()) ,2)

        # self.paint.pw.setXRange(min(x1,x2,y1,y2)-20, max(x1,x2,y1,y2)+20)
        # self.paint.pw.setXRange((min(x1,x2,y1,y2)-20)/1000, (max(x1,x2,y1,y2)+20)/1000)
        # self.paint.pw.setYRange((min(x1,x2,y1,y2)-20)/1000, (max(x1,x2,y1,y2)+20)/1000)
    def fuc_agv_resetPos(self):
        if(self.check_mode_retransfer.isChecked()):
            self.input_agv_currentPosX.setText(str(0))
            self.input_agv_currentPosY.setText(str(0))
        else:#控制板控制
            cmd_id=[0x40,0x35]
            cmd_data=[0x00,0x00]
            self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_agv_autoStop(self):
        self.fuc_2motor_setSpeed(0)
        self.timer1_agv_stop.stop()#停止运动只需触发一次，用于延时停       
        self.timer5_agv_angleJudge.stop()#旋转到位后停止判断
        self.timer6_agv_odometer.stop()#停止里程计
        self.timer7_agv_toDistance.stop()#中止运行指定距离
        if(self.agv_flag_moveCycle):  #往复运动模式
            countCycle=int(self.input_2motor_timeCycle.text())                
            if(self.agv_countCycle/2<countCycle):
                self.agv_countCycle+=1
                print(self.agv_countCycle)
                self.label_2motor_moveCycle.setText('当前第'+str(self.agv_countCycle/2)+'次循环')
                print(self.agv_countCycle/2<countCycle)
                if(self.agv_moveState=="forward"):
                    self.agv_moveState="waitBack"
                elif(self.agv_moveState=="back"):
                    self.agv_moveState="waitForward" 
                self.timer2_agv_moveCycle.start(3000)
            else:self.agv_countCycle=0;self.agv_flag_moveCycle=0;self.agv_moveState="stop"#往复运动次数到达清各种标志
        if(self.agv_moveState=="toPos1_rotate"):#前往指定点的第一步旋转指定角度完成
            self.agv_moveState="toPos2_forward"
            self.timer7_agv_toDistance.start(1000)
        elif(self.agv_moveState=="toPos2_forward"):#前往指定点的第二步运行指定距离完成
            self.agv_moveState="stop"
    def fuc_agv_handStop(self):
        if(self.check_mode_retransfer.isChecked()):
            #切换状态
            self.agv_moveState="stop"
            #下发指令
            self.fuc_2motor_setSpeed(0)
            #关闭定时触发器
            self.timer1_agv_stop.stop()#停止运动只需触发一次，用于延时停     
            self.timer2_agv_moveCycle.stop()
            self.timer5_agv_angleJudge.stop()#停止判断旋转角度
            self.timer6_agv_odometer.stop()#停止里程计
            self.timer7_agv_toDistance.stop()#中止运行到指定距离
        else:#控制板控制           
            cmd_id=[0x40,0x31]
            cmd_data=[0x00]
            self.fuc_serial_send(cmd_id,cmd_data)

    def fuc_agv_cycleMove(self):
        self.timer2_agv_moveCycle.stop()#延时后进入循环，进入循环后停止计时；用于电机到位后停止一段时间再继续运动
        self.agv_flag_moveCycle=1
        if(self.agv_moveState=="stop"):
            # print(self.agv_moveState)
            self.agv_moveState="forward"
            self.fuc_agv_moveForward()
        if(self.agv_moveState=="waitForward"):
            self.agv_moveState="forward"
            self.fuc_agv_moveForward()
        if(self.agv_moveState=="waitBack"):
            self.agv_moveState="back"
            self.fuc_agv_moveBack()
    def fuc_agv_stopCycle(self):
        self.fuc_2motor_setSpeed(0)
        self.timer2_agv_moveCycle.stop()
        self.timer1_agv_stop.stop()
        self.agv_flag_moveCycle=0
        self.agv_moveState="stop"
    def fuc_agv_speedChange(self,trig):#trig=1 rpm→v,0:v→rpm       
        reductionRatio=float(self.input_agv_parameter2_reductionRatio.text())#减速比
        wheelSize=float(self.input_agv_parameter1_wheelSize.text())#轮径
        if(trig):
            rpm=int(self.input_2motor_speed_1.text())
            v=rpm*wheelSize/60/reductionRatio*10*math.pi#转速/减速比*……
            v=int(v)
            self.input_agv_parameter3_realSpeed.setText(str(v))
        else:
            v=int(self.input_agv_parameter3_realSpeed.text())
            rpm=v*60*reductionRatio/wheelSize/10/math.pi
            rpm=int(rpm)
            rateSpeed=round(v/math.pi/self.agv_wheelDistance/10*360,2)#角速度=线速度/2*pi*R*360
            self.input_2motor_speed_1.setText(str(rpm))
            self.input_2motor_speed_2.setText(str(rpm))
            self.input_agv_parameter4_rateSpeed.setText(str(rateSpeed))

        #控制板控制
        speed=int(float(self.input_agv_parameter3_realSpeed.text()))
        print(speed)
        if(speed<0):speed=0x10000+speed
        cmd_id=[0x40,0x30]
        cmd_data=[speed>>8,speed&0xff]
        self.fuc_serial_send(cmd_id,cmd_data)
    #endregion
    
    #region # 无线手柄控制
    def fuc_handle_send(self,cmd_data):
        cmd_header = [0xAA, 0xAA]
        cmd_num=[0x01]
        cmd_length = [0x0D]
        cmd_voltage = [0xDF]
        sendData0=cmd_header+cmd_num+cmd_length+cmd_voltage+cmd_data
        sendData1=mycrc(sendData0)
        sendData2=bytes(sendData1)
        if self.check_mode_wifiToUart.isChecked():
            self.s.send(sendData2)  
        else:
            self.ser.write(sendData2)
        # self.signal.print.emit(HexStrAddSpace(data.hex())) 
        if(self.check_showsSendMessage.isChecked()):
            self.signalCall_printSendMessage(HexStrAddSpace(sendData2.hex()))
        
    def fuc_handle_LU(self):
        cmd_data=[0x7F,0x7F,0x7F,0x7F,0x01<<5,0x00]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_LL(self):
        cmd_data=[0x7F,0x7F,0x7F,0x7F,0x01<<4,0x00]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_LD(self):
        cmd_data=[0x7F,0x7F,0x7F,0x7F,0x01<<3,0x00]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_LR(self):
        cmd_data=[0x7F,0x7F,0x7F,0x7F,0x01<<2,0x00]
        self.fuc_handle_send(cmd_data)

    def fuc_handle_SE(self):
        cmd_data=[0x7F,0x7F,0x7F,0x7F,0x01<<1,0x00]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_ST(self):
        cmd_data=[0x7F,0x7F,0x7F,0x7F,0x01<<0,0x00]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_LD_pressed(self):
        print("fuc_handle_LD_pressed")
    
    
    
    def fuc_handle_RL(self):
        cmd_data=[0x7F,0x7F,0x7F,0x7F,0x00,0x01<<7]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_RD(self):
        cmd_data=[0x7F,0x7F,0x7F,0x7F,0x00,0x01<<6]
        self.fuc_handle_send(cmd_data)   
    def fuc_handle_RR(self):
        cmd_data=[0x7F,0x7F,0x7F,0x7F,0x00,0x01<<5]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_RU(self):
        cmd_data=[0x7F,0x7F,0x7F,0x7F,0x00,0x01<<4]
        self.fuc_handle_send(cmd_data)

    def fuc_handle_RK(self):
        cmd_data=[int(self.handle_AL1.value()),int(self.handle_AL2.value()),int(self.handle_AR1.value()),int(self.handle_AR2.value()),0x00,0x01<<1]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_LK(self):
        cmd_data=[int(self.handle_AL1.value()),int(self.handle_AL2.value()),int(self.handle_AR1.value()),int(self.handle_AR2.value()),0x00,0x01<<0]
        self.fuc_handle_send(cmd_data)

    def fuc_handle_AL1(self):
        cmd_data=[0xff-int(self.handle_AL1.value()),0xff-int(self.handle_AL2.value()),0xff-int(self.handle_AR1.value()),0xff-int(self.handle_AR2.value()),0x00,0x00]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_AL2(self):
        cmd_data=[0xff-int(self.handle_AL1.value()),0xff-int(self.handle_AL2.value()),0xff-int(self.handle_AR1.value()),0xff-int(self.handle_AR2.value()),0x00,0x00]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_AR1(self):
        cmd_data=[int(self.handle_AL1.value()),int(self.handle_AL2.value()),int(self.handle_AR1.value()),int(self.handle_AR2.value()),0x00,0x00]
        self.fuc_handle_send(cmd_data)
    def fuc_handle_AR2(self):
        cmd_data=[int(self.handle_AL1.value()),int(self.handle_AL2.value()),int(self.handle_AR1.value()),int(self.handle_AR2.value()),0x00,0x00]
        self.fuc_handle_send(cmd_data)
    #endregion
    #region # 多点路径规划   
    def fuc_agv_queryPoints(self):
        cmd_id=[0x40,0x50]
        cmd_data=[0x00,0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_setPoint(self):
        cmd_id=[0x40,0x51]
        x1=int(self.input_pointX_1.text()) 
        y1=int(self.input_pointY_1.text())
        x2=int(self.input_pointX_2.text())
        y2=int(self.input_pointY_2.text())
        x3=int(self.input_pointX_3.text())
        y3=int(self.input_pointY_3.text())
        x4=int(self.input_pointX_4.text())
        y4=int(self.input_pointY_4.text())
        if(x1<0):x1=0x10000+x1
        if(y1<0):y1=0x10000+y1
        if(x2<0):x2=0x10000+x2
        if(y2<0):y2=0x10000+y2
        if(x3<0):x3=0x10000+x3
        if(y3<0):y3=0x10000+y3
        if(x4<0):x4=0x10000+x4
        if(y4<0):y4=0x10000+y4
        print(hex(x2))
        cmd_data=[x1>>8&0xff,x1&0xff,y1>>8&0xff,y1&0xff,x2>>8&0xff,x2&0xff,y2>>8&0xff,y2&0xff,x3>>8&0xff,x3&0xff,y3>>8&0xff,y3&0xff,x4>>8&0xff,x4&0xff,y4>>8&0xff,y4&0xff]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_pointCycle(self):
        cmd_id=[0x40,0x52]
        route=int(self.com_route.currentText())
        if(self.check_pointCycle.isChecked()):
            cmd_data=[0x01,route]
        else:cmd_data=[0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_startPointRecycle(self):
        cmd_id=[0x40,0x36]
        cmd_data=[0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
    #endregion
    #region # 地图规划    
    def fuc_add_mapPoint(self):
        self.map.flag_editPoint=1
    def fuc_remove_mapPoint(self):
        self.map.flag_editPoint=2
    def fuc_edit_mapPoint(self):
        self.map.flag_editPoint=3
    def fuc_save_mapPoints(self):
        filename, _ = QFileDialog.getOpenFileName(None, 'Open File', os.getcwd()+"/mapPoints")
        with open(filename, "w",encoding='utf-8') as f:
            json.dump(self.map.dotdata, f)
    def fuc_load_mapPoints(self):
        filename, _ = QFileDialog.getOpenFileName(None, 'Open File', os.getcwd()+"/mapPoints")
        with open(filename, "r",encoding='utf-8') as f:
            content = f.read()
        self.map.dotdata=json.loads(content)
        self.map.scatter.setData(self.map.dotdata)
    def fuc_clear_mapPoints(self):
        self.map.dotdata=[]
        self.map.scatter.setData(self.map.dotdata)
    def fuc_run_mapPoints(self,direction):
        self.map.dotdataNum=len(self.map.dotdata)
        self.map.direction=direction
        if direction:
            self.map.dotdataCnt=0
        else:
            self.map.dotdataCnt=self.map.dotdataNum-1
        if self.map.dotdataNum:           
            x1=float(self.input_agv_currentPosX.text())
            y1=float(self.input_agv_currentPosY.text())
            x2=self.map.dotdata[self.map.dotdataCnt]['pos'][0]
            y2=self.map.dotdata[self.map.dotdataCnt]['pos'][1]
            print(x1,y1,x2,y2)
            self.flag_xInPos=0
            self.flag_yInPos=0
            if x1<=x2:
                self.flag_xInPos=1
            if y1<=y2:
                self.flag_yInPos=1
            print(self.flag_xInPos,self.flag_yInPos)

            cmd_id=[0x40,0x34]
            agv_targetPosX= floatToList(x2)
            agv_targetPosY= floatToList(y2)
            cmd_data=agv_targetPosX+agv_targetPosY
            self.fuc_serial_send(cmd_id,cmd_data)
            # self.map.dotdata=[]
            # self.map.scatter.setData(self.map.dotdata)
            self.timer11_update_mapPoints.start(300)
    def task_update_mapPoints(self):
        # print("task_update_mapPoints")
        if (self.map.direction and self.map.dotdataCnt<self.map.dotdataNum) or (self.map.direction==0 and self.map.dotdataCnt>=0):  
            x1=float(self.input_agv_currentPosX.text())
            y1=float(self.input_agv_currentPosY.text())
            x2=self.map.dotdata[self.map.dotdataCnt]['pos'][0]
            y2=self.map.dotdata[self.map.dotdataCnt]['pos'][1]
            err=0.02
            if(((self.flag_xInPos==1)&(x1>=x2-err))|((self.flag_xInPos==0)&(x1<=x2+err))and((self.flag_yInPos==1)&(y1>=y2-err))|((self.flag_yInPos==0)&(y1<=y2+err))):
                if self.map.direction:
                    self.map.dotdataCnt+=1
                else:
                    self.map.dotdataCnt-=1
                if (self.map.direction and self.map.dotdataCnt<self.map.dotdataNum) or (self.map.direction==0 and self.map.dotdataCnt>=0):         
                    x1=float(self.input_agv_currentPosX.text())
                    y1=float(self.input_agv_currentPosY.text())
                    x2=self.map.dotdata[self.map.dotdataCnt]['pos'][0]
                    y2=self.map.dotdata[self.map.dotdataCnt]['pos'][1]
                    
                    self.flag_xInPos=0
                    self.flag_yInPos=0
                    if x1<=x2:
                        self.flag_xInPos=1
                    if y1<=y2:
                        self.flag_yInPos=1

                    cmd_id=[0x40,0x34]
                    agv_targetPosX= floatToList(x2)
                    agv_targetPosY= floatToList(y2)
                    cmd_data=agv_targetPosX+agv_targetPosY
                    self.fuc_serial_send(cmd_id,cmd_data)
                    print(agv_targetPosX,agv_targetPosY)
    #endregion
    #region # 运动轨迹
    def fuc_clear_points(self):
        self.paint.dotdata=[]
        self.paint.scatter.setData(self.paint.dotdata)

        self.last_agv_currentPosX=1
        self.last_agv_currentPosY=1
        self.keyDots=[]

    def fuc_saveTrajectory(self,text):
        print(self.paint.dotdata)
        filename, _ = QFileDialog.getOpenFileName(None, 'Open File', os.getcwd()+"/trajectory")
        with open(filename, "w",encoding='utf-8') as f:
            json.dump(self.paint.dotdata, f)

    def fuc_loadTrajectory(self,text):
        filename, _ = QFileDialog.getOpenFileName(None, 'Open File', os.getcwd()+"/trajectory")
        with open(filename, "r",encoding='utf-8') as f:
            content = f.read()
        self.paint.dotdata=json.loads(content)
        self.paint.scatter.setData(self.paint.dotdata)

    def fuc_playTrajectory(self,direction):
        self.direction_playTrajectory=direction
        self.dotdata=[]
        self.paint.scatter.setData(self.dotdata)#清空点
        self.points_number=len(self.paint.dotdata)#轨迹中点的数量
        if direction:
            self.point_count=0#从第0个点开始
        else:
            self.point_count=self.points_number-1#从最后一个点开始
        self.timer9_playPoints.start(300)#每300ms打印1个点

    def task_playPoints(self):
        if self.direction_playTrajectory:
            if self.point_count<self.points_number:
                self.dotdata.append(self.paint.dotdata[self.point_count])
                self.paint.scatter.setData(self.dotdata)
                self.point_count+=1
            else:
                self.timer9_playPoints.stop()
        else:
            if self.point_count>=0:
                self.dotdata.append(self.paint.dotdata[self.point_count])
                self.paint.scatter.setData(self.dotdata)
                self.point_count-=1
            else:
                self.timer9_playPoints.stop()
    
    def add_keyDot(self,lineSpeed,rateSpeed):
        PosX=float(self.input_agv_currentPosX.text())
        PosY=float(self.input_agv_currentPosY.text())

        #新增红色关键点
        old_value ={'pos': (PosX, PosY)}
        new_value ={'pos': (PosX, PosY),'brush': 'r','pen':'r','data':[lineSpeed,rateSpeed]}
        self.paint.dotdata = [new_value if x == old_value else x for x in self.paint.dotdata]  
        self.paint.scatter.setData(self.paint.dotdata)

        self.keyDots.append(new_value)

    def fuc_save_keyDot(self,text):
        print(self.keyDots)
        filename, _ = QFileDialog.getOpenFileName(None, 'Open File', os.getcwd()+"/trajectory")
        with open(filename, "w",encoding='utf-8') as f:
            json.dump(self.keyDots, f)

    def fuc_load_keyDot(self,text):
        filename, _ = QFileDialog.getOpenFileName(None, 'Open File', os.getcwd()+"/trajectory")
        with open(filename, "r",encoding='utf-8') as f:
            content = f.read()
        self.keyDots=json.loads(content)
        self.paint.scatter.setData(self.keyDots)

    #endregion
    #region # 配置雷达避障
    def fuc_obstacle_ignore(self):
        cmd_id=[0x40,0x55]
        cmd_data=[0x00,0x00]
        if(self.check_obstacle_ignore.isChecked()):
            cmd_data=[0x01,0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
    #endregion
    #region # 配置IMU（陀螺仪）
    def fuc_imu_config(self,note):
        cmd_id=[0x40,0x09]
        CanId=[0x06,int(self.input_imu_nodeId.text())]
        if(note=="queryAngle"):
            CanData=[0x40,0x00,0x20,0x01,0x00,0x00,0x00,0x00] 
        if(note=="NMT"):# NMT（Network Management 网络管理）
            CanId=[0x00,0x00]
            NMT=[0x01,0x02,0x80,0x81,0x82] 
            CanData=[NMT[self.com_imu_NMT.currentIndex()],int(self.input_imu_nodeId.text())]
            print('hlly')
        cmd_data=CanId+CanData
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_imu_preConfigPDO(self,note,PDOid):
        cmd_id=[0x40,0x09]
        CanId=[0x06,int(self.input_imu_nodeId.text())] 
        if(note=="PDO_forbid"):#1.禁止PDO 
            CanData=[0x23,PDOid,0x18,0x01,0x82,0x01,0x00,0x80]
        if(note=="PDO_mode"):#2.配置TPDO1为异步
            CanData=[0x2F,PDOid,0x18,0x02,0xFF,0x00,0x00,0x00]
        if(note=="PDO_forbidTime"):#3.设置异步模式下的禁止时间
            CanData=[0x2F,PDOid,0x18,0x03,0x10,0x27,0x00,0x00]
        if(note=="PDO_time"):#4.设置异步模式下TPDO1的发送数据时间间隔
            time_interval=int(self.input_imu_timeTPO1.text())
            CanData=[0x2F,PDOid,0x18,0x05,time_interval&0xff,time_interval>>8,0x00,0x00]
        if(note=="PDO_clear"):#5.清空1A00的映射PDO个数
            CanData=[0x2F,PDOid,0x1A,0x00,0x00,0x00,0x00,0x00]
        if(note=="PDO_address_1"):#6.把2000:01映射到”1A00：01”
            CanData=[0x23,PDOid,0x1A,0x01,0x20,0x01,0x00,0x20]
        if(note=="PDO_address_2"):#6.把2000:02到”1A00：02”
            CanData=[0x23,PDOid,0x1A,0x02,0x20,0x02,0x00,0x20]
        if(note=="PDO_numer"):#7.设置1A00的映射PDO个数
            CanData=[0x2F,PDOid,0x1A,0x00,0x02,0x00,0x00,0x00]
        if(note=="PDO_enable"):#8.使能PDO 
            CanData=[0x23,PDOid,0x18,0x01,0x82,0x01,0x00,0x00]
        cmd_data=CanId+CanData
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_imu_configPDO(self,PDOid): 
        self.fuc_imu_preConfigPDO("PDO_forbid",PDOid)
        time.sleep(0.03)
        self.fuc_imu_preConfigPDO("PDO_mode",PDOid)
        time.sleep(0.03)
        self.fuc_imu_preConfigPDO("PDO_forbidTime",PDOid)
        time.sleep(0.03)
        self.fuc_imu_preConfigPDO("PDO_time",PDOid)
        time.sleep(0.03)
        self.fuc_imu_preConfigPDO("PDO_clear",PDOid)
        time.sleep(0.03)
        self.fuc_imu_preConfigPDO("PDO_address_1",PDOid)
        time.sleep(0.03)
        self.fuc_imu_preConfigPDO("PDO_address_2",PDOid)
        time.sleep(0.03)
        self.fuc_imu_preConfigPDO("PDO_numer",PDOid)
        time.sleep(0.03)
        self.fuc_imu_preConfigPDO("PDO_enable",PDOid)
    def fuc_imu_queryConstant(self):
        if(self.check_imu_queryConstant.isChecked()): self.timer4_imu_query.start(int(self.input_imu_timeQuery.text()))
        else: self.timer4_imu_query.stop()
    def fuc_imu_zero(self):
        cmd_id=[0x40,0x21]
        cmd_data=[0x00,0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_angleRectify(self):
        cmd_id=[0x40,0x22]
        angle=int(float(self.input_angleRectify.text())*10)
        cmd_data=[angle>>8,angle&0xff]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_imu_setOffset(self):
        cmd_id=[0x40,0x23]
        offset=floatToList(float(self.input_imu_offset.text()))
        cmd_data=offset
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_imu_setIntegralGain(self):
        cmd_id=[0x40,0x24]
        integralGain=floatToList(float(self.input_imu_integralGain.text()))
        cmd_data=integralGain
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_imu_test(self,cw):
        if(self.flag_imu_test):
            self.flag_imu_test=0
            self.timer10_imuTest.stop()
        else:         
            self.flag_imu_test=1
            self.angle_count=1
            self.imu_rotateCount=0
            if cw==1:
                self.angleList=[0,90,180,-90]
            elif cw==0:
                self.angleList=[0,-90,-180,90]
            elif cw==2:
                self.angleList=[0,90,0,-90]
            self.task_imuTest()
            self.timer10_imuTest.start(5000)
    def task_imuTest(self):
        self.input_agv_targetAngle.setText(str(self.angleList[self.angle_count]))
        self.fuc_agv_toAngle()
        self.angle_count+=1
        if(self.angle_count>3):self.angle_count=0

        self.imu_rotateCount+=1
        self.input_imu_rotateCount.setText(str(self.imu_rotateCount))
    #endregion
    #region # 查询读码相机
    def fuc_camera_query(self):
        cmd_id=[0x40,0x20]
        cmd_data=[0xC8,0x37]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_camera_queryConstant(self):
        if(self.check_camera_queryConstant.isChecked()): self.timer3_camera_query.start(int(self.input_camera_timeQuery.text()))
        else: self.timer3_camera_query.stop()
    #endregion
    #region # 控制板恢复出厂/重启/flash参数设置
    def fuc_system_reset(self):
        cmd_id=[0x10,0x00]
        cmd_data=[0x00,0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_system_restart(self):
        cmd_id=[0x10,0x01]
        cmd_data=[0x00,0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_save_flashParameter(self):
        cmd_id=[0x10,0x05]
        cmd_data=[0x00,0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
    def fuc_read_flashParameter(self):
        cmd_id=[0x10,0x02]
        cmd_data=[0x00,0x00]
        self.fuc_serial_send(cmd_id,cmd_data)
        self.btn_read_flashParameter.setText("读取中……")
    def fuc_set_flashParameter(self):
        cmd_id=[0x10,0x03]
        scale_rateToLine     =floatToList(float(self.input_scale_rateToLine     .text()))
        agv_default_lineSpeed=floatToList(float(self.input_agv_default_lineSpeed.text()))
        agv_default_rateSpeed=floatToList(float(self.input_agv_default_rateSpeed.text()))
        agv_acceleration     =floatToList(float(self.input_agv_acceleration     .text()))
        agv_deceleration     =floatToList(float(self.input_agv_deceleration     .text()))
        agv_RateAcceleration =floatToList(float(self.input_agv_RateAcceleration .text()))
        agv_RateDeceleration =floatToList(float(self.input_agv_RateDeceleration .text()))
        cmd_data=scale_rateToLine+agv_default_lineSpeed+agv_default_rateSpeed+agv_acceleration+agv_deceleration+agv_RateAcceleration+agv_RateDeceleration+[0x01]
        self.fuc_serial_send(cmd_id,cmd_data)
        time.sleep(0.03)
        imu_integralGain     =floatToList(float(self.input_imu_integralGain     .text()))
        imu_GyroOffset_Z=floatToList(float(self.input_imu_offset.text()))
        agv_angleRangeOffset=floatToList(float(self.input_angleRangeOffset.text()))
        agv_distanceRangeOffset=floatToList(float(self.input_distanceRangeOffset.text()))
        factor_rectify=floatToList(float(self.input_factor_rectify.text()))
        rectifyAngle=floatToList(float(self.input_rectifyAngle.text()))
        cmd_data=imu_integralGain+imu_GyroOffset_Z+agv_angleRangeOffset+agv_distanceRangeOffset+factor_rectify+rectifyAngle+agv_RateDeceleration+[0x02]
        self.fuc_serial_send(cmd_id,cmd_data)
    #endregion
# 子窗口
class Child(QMainWindow,Ui_child1.Ui_MainWindow):
  def __init__(self):
    super(Child, self).__init__()
    self.setupUi(self)
    self.setWindowTitle("AGV调试上位机")
    self.setWindowIcon(QIcon(r"E:\python\上位机_舜宇贝尔\控制板调试\sunny.png"))
    # self.pushButton.clicked.connect(self.close)
  def OPEN(self):
    self.show()
class Window(QMainWindow):
    def __init__(self):
        super().__init__()

if __name__ == "__main__":
    app = QApplication(sys.argv)
    # apply_stylesheet(app, theme='dark_teal.xml')

    client_sockets = []
    # MainWindow = QtWidgets.QMainWindow()
    # MainWindow = Window()
    MainWindow = Main()
    # ui = Main(MainWindow)
    # MainWindow.setWindowTitle("AGV调试上位机")
    # MainWindow.setWindowIcon(QIcon(r"src\pic/sunny.png"))
    # MainWindow.setWindowIcon(QIcon(''))  # 隐藏左上角的程序图标
    # MainWindow.setWindowIcon(QIcon("sunny.png"))
    MainWindow.show()

    # main = Main()    
    # main.show()   

    sys.exit(app.exec())
    
